Skip to content

Load multiple .xosc and change without restarting ATOS #682

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
samuelthoren wants to merge 2 commits into
base: dev
Choose a base branch
from
Open

Load multiple .xosc and change without restarting ATOS #682

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
78a8584
Add possibility to load multiple scenarios and change scenario withou…
samuelthoren May 4, 2026
28c6e3a
Some fixes
samuelthoren May 5, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions atos/modules/EsminiAdapter/src/esminiadapter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ EsminiAdapter::EsminiAdapter() :
oscFilePathClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
objectIdsClient_cb_group_ = this->create_callback_group(rclcpp::CallbackGroupType::Reentrant);
oscFilePathClient_ = create_client<atos_interfaces::srv::GetOpenScenarioFilePath>(
ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_);
ServiceNames::getOpenScenarioFilePath, rmw_qos_profile_services_default, oscFilePathClient_cb_group_);
objectIdsClient_ = create_client<atos_interfaces::srv::GetObjectIds>(
ServiceNames::getObjectIds, rmw_qos_profile_services_default, objectIdsClient_cb_group_);
declare_parameter("timestep", 0.1);
Expand Down Expand Up @@ -573,7 +573,7 @@ void EsminiAdapter::runEsminiSimulation() {

void EsminiAdapter::onRequestObjectTrajectory(const std::shared_ptr<ObjectTrajectorySrv::Request> req,
std::shared_ptr<ObjectTrajectorySrv::Response> res) {
res->id;
res->id = req->id;
me->fetchOSCFilePath();
if (me->runSimulation || me->atosObjectIdToTraj.find(req->id) == me->atosObjectIdToTraj.end()) {
me->runEsminiSimulation();
Expand Down
5 changes: 3 additions & 2 deletions atos/modules/ObjectControl/inc/statemachine.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -176,8 +176,9 @@ class StateMachine {
Connecting + sml::event<events::Ready> = Ready,

Ready + sml::on_entry<sml::_> / ready_on_entry(),
Ready + sml::event<events::Abort> = Aborting,
Ready + sml::event<events::Arm> = Armed,
Ready + sml::event<events::Abort> = Aborting,
Ready + sml::event<events::Arm> = Armed,
Ready + sml::event<events::Connect> / ready_reload(),
Ready + sml::event<events::Disconnect> = Idle,
Ready + sml::event<events::DisconnectedFromObject> / disconnected_from_object() = Connecting,
Ready + sml::event<events::RemoteControl> = RemoteControl,
Expand Down
49 changes: 38 additions & 11 deletions atos/modules/ObjectControl/src/objectcontrol.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -675,20 +675,47 @@ void ObjectControl::resetTestObjects() {

/**
* @brief Reloads the scenario trajectories for all objects
*
* @param isResetting state-flag to indicate if the reset procedure is currently running
*/
void ObjectControl::reloadScenarioTrajectories() {
// Check if we already have loaded the scenario trajectories
if (!this->isResetting) {
RCLCPP_INFO(get_logger(), "Scenario trajectories already loaded");
return;
}
this->isResetting = false;
RCLCPP_INFO(get_logger(), "Reloading scenario trajectories");
// Request the scenario trajectory for each object
for (auto& id : getVehicleIDs()) {
this->setObjectTrajectory(id);
auto trajRequest = std::make_shared<atos_interfaces::srv::GetObjectTrajectory::Request>();
trajRequest->id = id;
trajectoryClient->async_send_request(
trajRequest,
[this, id](const rclcpp::Client<atos_interfaces::srv::GetObjectTrajectory>::SharedFuture future) {
auto response = future.get();
if (!response->success) {
RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object ID %u", id);
return;
}
ATOS::Trajectory traj(get_logger());
traj.initializeFromCartesianTrajectory(response->trajectory);
objects.at(id)->setTrajectory(traj);

// Update origin (might be different .xodr for different .xosc)
auto originRequest = std::make_shared<atos_interfaces::srv::GetTestOrigin::Request>();
originClient->async_send_request(
originRequest,
[this, id, traj](const rclcpp::Client<atos_interfaces::srv::GetTestOrigin>::SharedFuture originFuture) {
auto originResponse = originFuture.get();
objects.at(id)->setOrigin({originResponse->origin.position.latitude,
originResponse->origin.position.longitude,
originResponse->origin.position.altitude,
true,
true,
true});
RCLCPP_INFO(get_logger(),
"Updated origin for object ID %u: (%.6f, %.6f, %.3f)",
id,
originResponse->origin.position.latitude,
originResponse->origin.position.longitude,
originResponse->origin.position.altitude);
objects.at(id)->sendTrajectory();
this->republishTrajectoryPaths(id);
RCLCPP_INFO(get_logger(), "Reloaded trajectory for object ID %u with %lu points", id, traj.size());
});
});
}
}

Expand Down Expand Up @@ -719,7 +746,7 @@ void ObjectControl::republishTrajectoryPaths(uint32_t id) {
void ObjectControl::trajectoryCallback(
const rclcpp::Client<atos_interfaces::srv::GetObjectTrajectory>::SharedFuture future) {
this->trajResponse = future.get();
auto id = returnTrajResponse->id;
auto id = trajResponse->id;
// Check if the return trajectory service call was successful
if (!trajResponse->success) {
RCLCPP_ERROR(get_logger(), "Get trajectory service call failed for object %u", id);
Expand Down
83 changes: 64 additions & 19 deletions atos/modules/OpenScenarioGateway/openscenariogateway.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
from rcl_interfaces.msg import SetParametersResult
from rclpy.node import Node
from scenariogeneration import xosc
from std_msgs.msg import Empty
from std_msgs.msg import Empty, String

import atos_interfaces.msg
import atos_interfaces.srv
Expand All @@ -25,6 +25,12 @@
RUNNING = 2


class ScenarioData:
def __init__(self, start_actions_to_obj_name: dict, custom_command_map: dict):
self.start_actions_to_obj_name = start_actions_to_obj_name
self.custom_command_map = custom_command_map


class ScenarioObject:
def __init__(self, name: str, catalog_ref: xosc.CatalogReference):
self.name = name
Expand All @@ -40,14 +46,16 @@ def __init__(self):
# Class variables
self.active_objects = {}
self.vehicle_catalog = None
self.start_actions_to_obj_name = {}
self.custom_command_map = {}
self.scenarios: dict[str, ScenarioData] = {}
self.active_scenario_file: str = ""

self.scenario_file_md5hash = None

# ROS parameters
self.declare_parameter(ROOT_FOLDER_PATH_PARAMETER, DEFAULT_FOLDER_PATH)
self.declare_parameter(SCENARIO_FILE_PARAMETER, "")
self.declare_parameter(
SCENARIO_FILE_PARAMETER, rclpy.Parameter.Type.STRING_ARRAY
)
self.declare_parameter(
ACTIVE_OBJECT_NAME_PARAMETER, rclpy.Parameter.Type.STRING_ARRAY
)
Expand All @@ -57,6 +65,9 @@ def __init__(self):
# ROS subscriptions/publishers
self.init_ = self.create_subscription(Empty, "init", self.init_callback, 10)
self.arm_ = self.create_subscription(Empty, "arm", self.arm_callback, 10)
self.active_scenario_sub = self.create_subscription(
String, "active_scenario", self.active_scenario_callback, 10
)

self.story_board_element_sub_ = self.create_subscription(
atos_interfaces.msg.StoryBoardElementStateChange,
Expand Down Expand Up @@ -90,7 +101,13 @@ def __init__(self):
)

def init_callback(self, msg):
self.update_scenario(self.get_parameter(SCENARIO_FILE_PARAMETER).value)
self.scenarios = {}
for file_name in self.get_parameter(SCENARIO_FILE_PARAMETER).value:
self.update_scenario(file_name)
# Default to the first scenario in the list
files = self.get_parameter(SCENARIO_FILE_PARAMETER).value
if not self.active_scenario_file and files:
self.active_scenario_file = files[0]
self.update_active_scenario_objects(
self.get_parameter(ACTIVE_OBJECT_NAME_PARAMETER).value
)
Expand All @@ -101,21 +118,27 @@ def arm_callback(self, msg):
self.active_objects[id].started = False

def story_board_element_state_change_callback(self, story_board_element):
active_scenario = self.active_scenario
if active_scenario is None:
self.get_logger().error(
"Received story board element state change but no active scenario is set"
)
return
if (
story_board_element.name in self.start_actions_to_obj_name.keys()
story_board_element.name in active_scenario.start_actions_to_obj_name.keys()
and story_board_element.state == RUNNING
):
self.handle_start_actions(story_board_element)
elif (
story_board_element.full_path in self.custom_command_map
story_board_element.full_path in active_scenario.custom_command_map
and story_board_element.state == RUNNING
):
self.handle_custom_command_action(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.start_actions_to_obj_name[
target_object_name = self.active_scenario.start_actions_to_obj_name[
story_board_element.name
]
if object.name in target_object_name and not object.started:
Expand All @@ -140,18 +163,29 @@ def handle_custom_command_action(self, story_board_element):
story_board_element.full_path
)
)
custom_command = self.custom_command_map[story_board_element.full_path]
custom_command = self.active_scenario.custom_command_map[
story_board_element.full_path
]

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 active_scenario_callback(self, msg: String):
if msg.data in self.scenarios:
self.active_scenario_file = msg.data
self.get_logger().info(f"Active scenario set to: {msg.data}")
else:
self.get_logger().warn(f"Received unknown scenario '{msg.data}'")

def parameter_callback(self, params):
for param in params:
if param.name == SCENARIO_FILE_PARAMETER and param.value:
self.update_scenario(file_name=param.value)
self.scenarios = {}
for file_name in param.value:
self.update_scenario(file_name=file_name)
elif param.name == ACTIVE_OBJECT_NAME_PARAMETER:
self.update_active_scenario_objects(active_objects_name=param.value)
return SetParametersResult(successful=True)
Expand All @@ -163,12 +197,14 @@ 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.start_actions_to_obj_name = StoryBoardHandler(
scenario_file
).get_follow_trajectory_actions_to_actors_map()
self.custom_command_map = StoryBoardHandler(
self.getAbsoluteOSCPath(file_name)
).get_custom_command_actions_map()
self.scenarios[file_name] = ScenarioData(
start_actions_to_obj_name=StoryBoardHandler(
scenario_file
).get_follow_trajectory_actions_to_actors_map(),
custom_command_map=StoryBoardHandler(
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)):
Expand Down Expand Up @@ -257,10 +293,19 @@ def srv_get_open_scenario_file_path(self, request, response):
response.success = True
return response

@property
def active_scenario_file_name(self) -> str:
if self.active_scenario_file and self.active_scenario_file in self.scenarios:
return self.active_scenario_file
files = self.get_parameter(SCENARIO_FILE_PARAMETER).value
return files[0] if files else ""

@property
def active_scenario(self) -> ScenarioData:
return self.scenarios.get(self.active_scenario_file_name)

def getScenarioFilePath(self) -> str:
return self.getAbsoluteOSCPath(
self.get_parameter(SCENARIO_FILE_PARAMETER).value
)
return self.getAbsoluteOSCPath(self.active_scenario_file_name)

def getAbsoluteOSCPath(self, file_name: str) -> str:
return path.join(
Expand Down
4 changes: 2 additions & 2 deletions atos/modules/OpenScenarioGateway/tests/test_openscenariogateway.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,8 +22,8 @@ def open_scenario_gw():
),
rclpy.parameter.Parameter(
"open_scenario_file",
rclpy.Parameter.Type.STRING,
"GaragePlanScenario.xosc",
rclpy.Parameter.Type.STRING_ARRAY,
["GaragePlanScenario.xosc"],
),
rclpy.parameter.Parameter(
"active_object_names", rclpy.Parameter.Type.STRING_ARRAY, active_objects
Expand Down
82 changes: 80 additions & 2 deletions atos_gui/atos_gui/controlpanel/controlpanel.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,10 @@

import rclpy
from nicegui import ui
from rcl_interfaces.msg import ParameterType
from rcl_interfaces.srv import GetParameters
from rclpy.node import Node
from std_msgs.msg import Empty
from std_msgs.msg import Empty, String

from atos_interfaces.srv import *

Expand Down Expand Up @@ -53,6 +55,17 @@ def __init__(self) -> None:
0.5, self.get_object_control_state_callback
)

self.scenario_names = []
self.selected_scenario = ""
self.scenario_select = None
self.active_scenario_pub = self.create_publisher(
String, "/atos/active_scenario", QOS
)
self.get_scenario_names_client = self.create_client(
GetParameters,
"/atos/open_scenario_gateway/get_parameters",
)

self.OBC_state = {"state": "UNDEFINED"}
self.lost_connection = True

Expand All @@ -68,7 +81,12 @@ def render_page():
).props("size=large ")
with ui.row():
ui.button(
"Init", on_click=lambda: self.initPub.publish(Empty()), color="blue"
"Init",
on_click=lambda: [
self.initPub.publish(Empty()),
self.fetch_scenario_names(),
],
color="blue",
)
ui.button(
"Connect",
Expand Down Expand Up @@ -98,6 +116,28 @@ def render_page():
on_click=lambda: self.allClearPub.publish(Empty()),
color="grey",
)
with ui.row().classes("items-center"):
with ui.element("div"):
self.scenario_select = (
ui.select(
options=self.scenario_names,
label="Active scenario",
on_change=lambda e: [
self.set_active_scenario(e.value),
self.connectPub.publish(Empty()),
],
)
.bind_value(self, "selected_scenario")
.bind_enabled_from(
self.OBC_state, "state", backward=lambda s: s == "CONNECTED"
)
.props("outlined dense")
)
ui.tooltip(
"Changing scenario can ony be done in CONNECTED state"
).bind_visibility_from(
self.OBC_state, "state", backward=lambda s: s != "CONNECTED"
)
with ui.row():
ui.label().bind_text_from(
self.OBC_state, "state", backward=lambda n: f"State: {n}"
Expand All @@ -114,6 +154,44 @@ def render_page():
color="grey",
)

def fetch_scenario_names(self) -> None:
if not self.get_scenario_names_client.wait_for_service(timeout_sec=1.0):
self.get_logger().warn(
"Failed to fetch scenario names: service not available"
)
return
req = GetParameters.Request()
req.names = ["open_scenario_file"]
self.get_scenario_names_client.call_async(req).add_done_callback(
self.on_scenario_names_fetched
)

def on_scenario_names_fetched(self, future) -> None:
try:
response = future.result()
except Exception as e:
self.get_logger().error(f"Failed to fetch scenario names: {e}")
return
if (
response.values
and response.values[0].type == ParameterType.PARAMETER_STRING_ARRAY
):
names = list(response.values[0].string_array_value)
self.scenario_names = names
if self.scenario_select is not None:
self.scenario_select.options = names
if not self.selected_scenario and names:
self.selected_scenario = names[0]
self.scenario_select.set_value(names[0])
self.scenario_select.update()

def set_active_scenario(self, scenario_name: str) -> None:
if not isinstance(scenario_name, str) or not scenario_name:
return
msg = String()
msg.data = scenario_name
self.active_scenario_pub.publish(msg)

def get_object_control_state_callback(self):
# Call the service
while not self.get_object_control_state_client.wait_for_service(
Expand Down
Loading
Loading