Add Rviz Marker for some conditions

openscenario/openscenario_interpreter/include/openscenario_interpreter/openscenario_interpreter.hpp

@@ -64,6 +64,8 @@ class Interpreter : public rclcpp_lifecycle::LifecycleNode,
 
   bool record;
 
+  bool publish_visualization;
+
   std::shared_ptr<OpenScenario> script;
 
   std::list<std::shared_ptr<ScenarioDefinition>> scenarios;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp

@@ -25,6 +25,7 @@
 #include <openscenario_interpreter/syntax/routing_algorithm.hpp>
 #include <openscenario_interpreter/syntax/rule.hpp>
 #include <openscenario_interpreter/syntax/triggering_entities.hpp>
+#include <openscenario_interpreter/visualization_buffer.hpp>
 #include <pugixml.hpp>
 #include <valarray>
 
@@ -60,7 +61,9 @@ inline namespace syntax
      <xsd:attribute name="routingAlgorithm" type="RoutingAlgorithm"/>
    </xsd:complexType>
 */
-struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvaluation
+struct DistanceCondition : private Scope,
+                           private SimulatorCore::ConditionEvaluation,
+                           private VisualizationBuffer::Target
 {
   /*
      Definition of the coordinate system to be used for calculations. If not
@@ -120,6 +123,8 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua
     throw SyntaxError(__FILE__, ":", __LINE__);
   }
 
+  void visualize() const;
+
   static auto evaluate(
     const Entities *, const Entity &, const Position &, CoordinateSystem, RelativeDistanceType,
     RoutingAlgorithm, Boolean) -> double;

openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp

@@ -21,6 +21,7 @@
 #include <openscenario_interpreter/syntax/position.hpp>
 #include <openscenario_interpreter/syntax/rule.hpp>
 #include <openscenario_interpreter/syntax/triggering_entities.hpp>
+#include <openscenario_interpreter/visualization_buffer.hpp>
 #include <pugixml.hpp>
 #include <valarray>
 
@@ -38,7 +39,8 @@ inline namespace syntax
  *  </xsd:complexType>
  *
  * -------------------------------------------------------------------------- */
-struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversion
+struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversion,
+                                private VisualizationBuffer::Target
 {
   const Double tolerance;
 
@@ -52,6 +54,8 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio
 
   explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &);
 
+  auto visualize() const -> void;
+
   auto description() const -> String;
 
   auto evaluate() -> Object;

openscenario/openscenario_interpreter/include/openscenario_interpreter/visualization_buffer.hpp

@@ -0,0 +1,88 @@
+// 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_INTERPRETER__VISUALIZATION_BUFFER_HPP_
+#define OPENSCENARIO_INTERPRETER__VISUALIZATION_BUFFER_HPP_
+
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+
+namespace openscenario_interpreter
+{
+class VisualizationBuffer
+{
+  static inline std::unique_ptr<VisualizationBuffer> buffer = nullptr;
+
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr publisher;
+  std::vector<visualization_msgs::msg::Marker> markers;
+
+public:
+  template <typename Node>
+  VisualizationBuffer(Node && node, const std::string topic = "/simulation/interpreter/markers")
+  {
+    publisher = rclcpp::create_publisher<visualization_msgs::msg::MarkerArray>(
+      node, topic, rclcpp::QoS(rclcpp::KeepLast(1)));
+  }
+
+  static auto active() { return static_cast<bool>(buffer); }
+
+  template <typename Node>
+  static auto activate(const Node & node) -> void
+  {
+    if (not active()) {
+      buffer = std::make_unique<VisualizationBuffer>(node);
+    } else {
+      throw Error("The visualization buffer has already been instantiated.");
+    }
+  }
+
+  static auto deactivate() -> void
+  {
+    if (active()) {
+      buffer.reset();
+    }
+  }
+
+  static auto flush() -> void
+  {
+    if (active()) {
+      visualization_msgs::msg::MarkerArray message;
+      message.markers = buffer->markers;
+      buffer->publisher->publish(message);
+      buffer->markers.clear();
+    }
+  }
+
+  class Target
+  {
+  public:
+    void add(const visualization_msgs::msg::Marker & marker) const
+    {
+      if (active()) {
+        buffer->markers.push_back(marker);
+      }
+    }
+
+    template <typename F>
+    void call_visualize(F && function) const
+    {
+      if (active()) {
+        function();
+      }
+    }
+  };
+};
+}  // namespace openscenario_interpreter
+
+#endif  // OPENSCENARIO_INTERPRETER__VISUALIZATION_BUFFER_HPP_

openscenario/openscenario_interpreter/package.xml

@@ -25,6 +25,7 @@
   <depend>tier4_simulation_msgs</depend>
   <depend>traffic_simulator</depend>
   <depend>traffic_simulator_msgs</depend>
+  <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>

openscenario/openscenario_interpreter/src/openscenario_interpreter.cpp

@@ -24,6 +24,7 @@
 #include <openscenario_interpreter/syntax/scenario_object.hpp>
 #include <openscenario_interpreter/syntax/speed_condition.hpp>
 #include <openscenario_interpreter/utility/overload.hpp>
+#include <openscenario_interpreter/visualization_buffer.hpp>
 #include <status_monitor/status_monitor.hpp>
 #include <traffic_simulator/data_type/lanelet_pose.hpp>
 
@@ -42,14 +43,16 @@ Interpreter::Interpreter(const rclcpp::NodeOptions & options)
   osc_path(""),
   output_directory("/tmp"),
   publish_empty_context(false),
-  record(false)
+  record(false),
+  publish_visualization(false)
 {
   DECLARE_PARAMETER(local_frame_rate);
   DECLARE_PARAMETER(local_real_time_factor);
   DECLARE_PARAMETER(osc_path);
   DECLARE_PARAMETER(output_directory);
   DECLARE_PARAMETER(publish_empty_context);
   DECLARE_PARAMETER(record);
+  DECLARE_PARAMETER(publish_visualization);
 
   declare_parameter<std::string>("speed_condition", "legacy");
   SpeedCondition::compatibility =
@@ -112,11 +115,13 @@ auto Interpreter::on_configure(const rclcpp_lifecycle::State &) -> Result
       GET_PARAMETER(output_directory);
       GET_PARAMETER(publish_empty_context);
       GET_PARAMETER(record);
+      GET_PARAMETER(publish_visualization);
 
       script = std::make_shared<OpenScenario>(osc_path);
 
-      // CanonicalizedLaneletPose is also used on the OpenScenarioInterpreter side as NativeLanePose.
-      // so canonicalization takes place here - it uses the value of the consider_pose_by_road_slope parameter
+      // CanonicalizedLaneletPose is also used on the OpenScenarioInterpreter side as
+      // NativeLanePose. so canonicalization takes place here - it uses the value of the
+      // consider_pose_by_road_slope parameter
       traffic_simulator::lanelet_pose::CanonicalizedLaneletPose::setConsiderPoseByRoadSlope([&]() {
         if (not has_parameter("consider_pose_by_road_slope")) {
           declare_parameter("consider_pose_by_road_slope", false);
@@ -183,6 +188,8 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
     withExceptionHandler(
       [this](auto &&...) {
         publishCurrentContext();
+        VisualizationBuffer::flush();
+        VisualizationBuffer::deactivate();
         deactivate();
       },
       [this]() {
@@ -204,6 +211,7 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
           SimulatorCore::update();
 
           publishCurrentContext();
+          VisualizationBuffer::flush();
         });
       });
   };
@@ -226,6 +234,10 @@ auto Interpreter::on_activate(const rclcpp_lifecycle::State &) -> Result
         SimulatorCore::activate(
           shared_from_this(), makeCurrentConfiguration(), local_real_time_factor, local_frame_rate);
 
+        if (publish_visualization) {
+          VisualizationBuffer::activate(shared_from_this());
+        }
+
         /*
            DIRTY HACK!
 

openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp

@@ -609,18 +609,105 @@ auto DistanceCondition::distance<
     position);
 }
 
+void DistanceCondition::visualize() const
+{
+  if (relative_distance_type != RelativeDistanceType::euclidianDistance) {
+    /// @todo implement visualization for other distance types
+    return;
+  }
+  auto center = static_cast<geometry_msgs::msg::Pose>(position);
+  center.orientation.w = 0;
+
+  const auto make_label_marker = [&](auto && triggering_entity) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = rclcpp::Clock().now();
+    marker.ns = "distance_condition/" + triggering_entity.name();
+    marker.id = 2;
+    marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose = center;
+    marker.pose.position.z += 0.3;
+    marker.text = description();
+    marker.scale.z = 0.3;
+    marker.color.a = 0.8f;
+    marker.color.r = 1.0;
+    marker.color.g = 1.0;
+    marker.color.b = 1.0;
+    return marker;
+  };
+
+  const auto make_distance_marker = [&](auto && triggering_entity) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = rclcpp::Clock().now();
+    marker.ns = "distance_condition/" + triggering_entity.name();
+    marker.id = 3;
+    marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    auto relative_pose = makeNativeRelativeWorldPosition(center, triggering_entity.name());
+    marker.points.push_back(center.position);
+    auto entity_position = center.position;
+    entity_position.x -= relative_pose.position.x;
+    entity_position.y -= relative_pose.position.y;
+    entity_position.z -= relative_pose.position.z;
+    marker.points.push_back(entity_position);
+    marker.scale.x = 0.1;
+    marker.color.a = 0.8f;
+    marker.color.r = 1.0;
+    marker.color.g = 0.0;
+    marker.color.b = 0.0;
+    return marker;
+  };
+
+  const auto make_distance_label_marker = [&](auto && triggering_entity) {
+    visualization_msgs::msg::Marker marker;
+    marker.header.frame_id = "map";
+    marker.header.stamp = rclcpp::Clock().now();
+    marker.ns = "distance_condition/" + triggering_entity.name();
+    marker.id = 4;
+    marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    auto relative_pose = makeNativeRelativeWorldPosition(center, triggering_entity.name());
+    marker.pose.position.x = center.position.x - relative_pose.position.x / 2;
+    marker.pose.position.y = center.position.y - relative_pose.position.y / 2;
+    marker.pose.position.z = center.position.z - relative_pose.position.z / 2;
+    marker.text = std::to_string(distance(triggering_entity));
+    marker.scale.z = 0.3;
+    marker.color.a = 0.8f;
+    marker.color.r = 1.0;
+    marker.color.g = 0.0;
+    marker.color.b = 0.0;
+    return marker;
+  };
+
+  std::for_each(
+    triggering_entities.entity_refs.begin(), triggering_entities.entity_refs.end(),
+    [&](auto && triggering_entity) {
+      triggering_entity.apply([&](auto && object) {
+        add(make_label_marker(object));
+        add(make_distance_marker(object));
+        add(make_distance_label_marker(object));
+      });
+    });
+}
+
 auto DistanceCondition::evaluate() -> Object
 {
   results.clear();
 
-  return asBoolean(triggering_entities.apply([&](const auto & triggering_entity) {
+  auto result = asBoolean(triggering_entities.apply([&](const auto & triggering_entity) {
     results.push_back(triggering_entity.apply([&](const auto & triggering_entity) {
       return evaluate(
         global().entities, triggering_entity, position, coordinate_system, relative_distance_type,
         routing_algorithm, freespace);
     }));
     return not results.back().size() or rule(results.back(), value).min();
   }));
+
+  call_visualize([this]() { visualize(); });
+
+  return result;
 }
 }  // namespace syntax
 }  // namespace openscenario_interpreter