feat(parameters_visualization): visualize test iteration parameters

openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp

@@ -33,6 +33,7 @@
 #include <scenario_simulator_exception/exception.hpp>
 #include <simple_junit/junit5.hpp>
 #include <utility>
+#include <std_msgs/msg/string.hpp>
 
 #define INTERPRETER_INFO_STREAM(...) \
   RCLCPP_INFO_STREAM(get_logger(), "\x1b[32m" << __VA_ARGS__ << "\x1b[0m")
@@ -50,7 +51,9 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode,
 {
   using Context = openscenario_interpreter_msgs::msg::Context;
 
+
   const rclcpp_lifecycle::LifecyclePublisher<Context>::SharedPtr publisher_of_context;
+  const rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>::SharedPtr publisher_of_params;
 
   double local_frame_rate;
 
@@ -79,6 +82,8 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode,
   using Result = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
   bool waiting_for_engagement_to_be_completed = false;  // NOTE: DIRTY HACK!!!
+
+  std_msgs::msg::String test_iteration_params_str = std_msgs::msg::String();
 
 public:
   OPENSCENARIO_INTERPRETER_PUBLIC

openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp

@@ -36,6 +36,7 @@ namespace openscenario_interpreter
 Interpreter::Interpreter(const rclcpp::NodeOptions & options)
 : rclcpp_lifecycle::LifecycleNode("openscenario_interpreter", options),
   publisher_of_context(create_publisher<Context>("context", rclcpp::QoS(1).transient_local())),
+  publisher_of_params(create_publisher<std_msgs::msg::String>("test_iteration_parameters", rclcpp::QoS(1).transient_local())),
   local_frame_rate(30),
   local_real_time_factor(1.0),
   osc_path(""),
@@ -122,6 +123,14 @@ auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result
 
       if (script->category.is<ScenarioDefinition>()) {
         scenarios = {std::dynamic_pointer_cast<ScenarioDefinition>(script->category)};
+        // get and log parameters used in this test iteration
+        test_iteration_params_str.data.clear();
+        test_iteration_params_str.data = "Parameter declarations for this scenario iteration:\n";
+        for (const auto & pd : currentScenarioDefinition()->parameter_declarations) {
+          test_iteration_params_str.data.append(pd.name + " = " + pd.value + "\n");
+        }
+        RCLCPP_INFO_STREAM(get_logger(), test_iteration_params_str.data);
+
       } else if (script->category.is<ParameterValueDistribution>()) {
         throw Error(
           "ParameterValueDistribution cannot be processed by openscenario_interpreter alone. "
@@ -175,10 +184,12 @@ auto Interpreter::engaged() const -> bool
 
 auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
 {
+
   auto evaluate_storyboard = [this]() {
     withExceptionHandler(
       [this](auto &&...) {
         publishCurrentContext();
+        publisher_of_params->publish(test_iteration_params_str);
         deactivate();
       },
       [this]() {
@@ -200,6 +211,7 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
           SimulatorCore::update();
 
           publishCurrentContext();
+          publisher_of_params->publish(test_iteration_params_str);
         });
       });
   };
@@ -237,8 +249,11 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
         execution_timer.clear();
 
         publisher_of_context->on_activate();
+        publisher_of_params->on_activate();
+
 
         assert(publisher_of_context->is_activated());
+        assert(publisher_of_params->is_activated());
 
         if (currentScenarioDefinition()) {
           currentScenarioDefinition()->storyboard.init.evaluateInstantaneousActions();
@@ -308,6 +323,11 @@ auto Interpreter::reset() -> void
     publisher_of_context->on_deactivate();
   }
 
+    if (publisher_of_params->is_activated()) {
+    publisher_of_params->on_deactivate();
+  }
+
+
   if (not has_parameter("initialize_duration")) {
     declare_parameter<int>("initialize_duration", 30);
   }

rviz_plugins/openscenario_visualization/CMakeLists.txt

@@ -47,6 +47,8 @@ ament_auto_add_library(openscenario_visualization_rviz_plugin SHARED
   src/openscenario_visualization_condition_groups_plugin/jsk_overlay_utils.cpp
   include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.hpp
   src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp
+  include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_parameters_plugin.hpp
+  src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_parameters_plugin.cpp
 )
 
 target_include_directories(openscenario_visualization_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include")

rviz_plugins/openscenario_visualization/include/openscenario_visualization_condition_groups_plugin/openscenario_visualization_parameters_plugin.hpp

@@ -0,0 +1,86 @@
+// Copyright 2015 TIER IV, Inc. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef OPENSCENARIO_VISUALIZATION_PARAMETERS_HPP_
+#define OPENSCENARIO_VISUALIZATION_PARAMETERS_HPP_
+
+#include <yaml-cpp/yaml.h>
+
+#include <memory>
+#include <nlohmann/json.hpp>
+
+#ifndef Q_MOC_RUN
+#include <OgreBillboardSet.h>
+#include <OgreManualObject.h>
+#include <OgreSceneManager.h>
+#include <OgreSceneNode.h>
+
+#include <openscenario_interpreter_msgs/msg/context.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rviz_common/display_context.hpp>
+#include <rviz_common/frame_manager_iface.hpp>
+#include <rviz_common/message_filter_display.hpp>
+#include <rviz_common/properties/bool_property.hpp>
+#include <rviz_common/properties/color_property.hpp>
+#include <rviz_common/properties/enum_property.hpp>
+#include <rviz_common/properties/float_property.hpp>
+#include <rviz_common/properties/int_property.hpp>
+#include <rviz_common/properties/string_property.hpp>
+#include <rviz_common/tool.hpp>
+#include <rviz_common/validate_floats.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include "jsk_overlay_utils.hpp"
+#endif
+
+namespace openscenario_visualization
+{
+
+class VisualizationParametersDisplay : public rviz_common::Display
+{
+  Q_OBJECT
+
+public:
+  VisualizationParametersDisplay();
+  virtual ~VisualizationParametersDisplay();
+
+  void onInitialize() override;
+  void onDisable() override;
+  void onEnable() override;
+  void subscribe();
+  void unsubscribe();
+
+private Q_SLOTS:
+  void updateTopic();
+  void updateVisualization();
+
+protected:
+  void processMessage(const std_msgs::msg::String::ConstSharedPtr msg_ptr);
+  jsk_rviz_plugins::OverlayObject::Ptr overlay_;
+  rviz_common::properties::ColorProperty * property_text_color_;
+  rviz_common::properties::IntProperty * property_left_;
+  rviz_common::properties::IntProperty * property_top_;
+  rviz_common::properties::IntProperty * property_length_;
+  rviz_common::properties::IntProperty * property_width_;
+  rviz_common::properties::StringProperty * property_topic_name_;
+  rviz_common::properties::FloatProperty * property_value_scale_;
+
+private:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr test_iteration_params_sub_;
+  std_msgs::msg::String::ConstSharedPtr last_msg_ptr_;
+};
+
+}  // namespace openscenario_visualization
+
+#endif  // OPENSCENARIO_VISUALIZATION_PARAMETERS_HPP_

rviz_plugins/openscenario_visualization/plugins.xml

@@ -7,4 +7,7 @@
   <class name="openscenario_visualization/VisualizationConditionGroups" type="openscenario_visualization::VisualizationConditionGroupsDisplay" base_class_type="rviz_common::Display">
     <description>Display Condition Groups</description>
   </class>
+  <class name="openscenario_visualization/VisualizationParameters" type="openscenario_visualization::VisualizationParametersDisplay" base_class_type="rviz_common::Display">
+    <description>Display Parameters</description>
+  </class>
 </library>