diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp index 2ec83694855..6f57435453b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp @@ -76,6 +76,8 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode, String record_storage_id; + String record_option; + std::shared_ptr script; std::list> scenarios; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/record.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/record.hpp index 85e1a638b33..681647fca04 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/record.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/record.hpp @@ -65,6 +65,36 @@ auto start(Ts &&... xs) -> pid_t } } +inline auto start_with_vector(const std::vector& args) -> pid_t +{ + std::vector command{ + "python3", boost::algorithm::replace_all_copy(concealer::dollar("which ros2"), "\n", ""), "bag", + "record"}; + + command.insert(command.end(), args.begin(), args.end()); + + switch (process_id = fork()) { + case -1: + throw std::system_error(errno, std::system_category()); + + case 0: +#ifndef OPENSCENARIO_INTERPRETER_VERBOSE_RECORD + if (const auto fd = ::open("/dev/null", O_WRONLY)) { + ::dup2(fd, STDOUT_FILENO); + ::close(fd); + } +#endif + if (concealer::execute(command) < 0) { + std::cerr << std::system_error(errno, std::system_category()).what() << std::endl; + std::exit(EXIT_FAILURE); + } + return 0; + + default: + return process_id; + } +} + auto stop() -> void; } // namespace record } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp index b2a8c54d9b8..68caa7a707f 100644 --- a/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp +++ b/openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp @@ -49,7 +49,8 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options) output_directory("/tmp"), publish_empty_context(false), record(false), - record_storage_id("") + record_storage_id(""), + record_option("") { DECLARE_PARAMETER(local_frame_rate); DECLARE_PARAMETER(local_real_time_factor); @@ -58,6 +59,7 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options) DECLARE_PARAMETER(publish_empty_context); DECLARE_PARAMETER(record); DECLARE_PARAMETER(record_storage_id); + DECLARE_PARAMETER(record_option); declare_parameter("speed_condition", "legacy"); SpeedCondition::compatibility = @@ -120,6 +122,7 @@ auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result GET_PARAMETER(publish_empty_context); GET_PARAMETER(record); GET_PARAMETER(record_storage_id); + GET_PARAMETER(record_option); script = std::make_shared(osc_path); @@ -260,16 +263,31 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result }, [&]() { if (record) { - if (record_storage_id == "") { - record::start( - "-a", "--exclude", "/sensing/camera/.*", "-o", - boost::filesystem::path(osc_path).replace_extension("").string()); - } else { - record::start( - "-a", "--exclude", "/sensing/camera/.*", "-o", - boost::filesystem::path(osc_path).replace_extension("").string(), "-s", - record_storage_id); + std::vector options; + options.push_back("-a"); + options.push_back("--exclude"); + options.push_back("/sensing/camera/.*"); + options.push_back("-o"); + options.push_back(boost::filesystem::path(osc_path).replace_extension("").string()); + if (record_storage_id != "") { + options.push_back("-s"); + options.push_back(record_storage_id); + } + if(record_option != ""){ + auto split = [](const std::string& s)->std::vector { + std::istringstream iss(s); + std::vector result; + std::string token; + + while (iss >> token) { + result.push_back(token); + } + return result; + }; + auto splitted = split(record_option); + options.insert(options.end(), splitted.begin(), splitted.end()); } + record::start_with_vector(options); } SimulatorCore::activate( diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index f4e2bfd656f..85880de6111 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -88,6 +88,7 @@ def launch_setup(context, *args, **kwargs): publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) record = LaunchConfiguration("record", default=True) record_storage_id = LaunchConfiguration("record_storage_id", default="") + record_option = LaunchConfiguration("record_option", default="") rviz_config = LaunchConfiguration("rviz_config", default=default_rviz_config_file()) scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) sensor_model = LaunchConfiguration("sensor_model", default="") @@ -116,6 +117,7 @@ def launch_setup(context, *args, **kwargs): print(f"publish_empty_context := {publish_empty_context.perform(context)}") print(f"record := {record.perform(context)}") print(f"record_storage_id := {record_storage_id.perform(context)}") + print(f"record_option := {record_option.perform(context)}") print(f"rviz_config := {rviz_config.perform(context)}") print(f"scenario := {scenario.perform(context)}") print(f"sensor_model := {sensor_model.perform(context)}") @@ -144,6 +146,7 @@ def make_parameters(): {"publish_empty_context" : publish_empty_context}, {"record": record}, {"record_storage_id": record_storage_id}, + {"record_option": record_option}, {"rviz_config": rviz_config}, {"sensor_model": sensor_model}, {"sigterm_timeout": sigterm_timeout},