Solves crash on RayQuery

delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.cc

@@ -600,6 +600,16 @@ const maliput::api::Lane* MaliputViewerModel::GetLaneFromWorldPosition(const ign
   return rg->ToRoadPosition(inertial_pos).road_position.lane;
 }
 
+///////////////////////////////////////////////////////
+const maliput::api::RoadPositionResult MaliputViewerModel::GetRoadPositionResult(
+    const ignition::math::Vector3d& _position) {
+  const maliput::api::RoadGeometry* rg =
+      this->roadGeometry ? this->roadGeometry.get() : this->roadNetwork->road_geometry();
+  DELPHYNE_DEMAND(rg != nullptr);
+  const maliput::api::InertialPosition inertial_pos(_position.X(), _position.Y(), _position.Z());
+  return rg->ToRoadPosition(inertial_pos);
+}
+
 ///////////////////////////////////////////////////////
 const maliput::api::Lane* MaliputViewerModel::GetLaneFromId(const std::string& _id) {
   const maliput::api::RoadGeometry* rg =

delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.hh

@@ -227,6 +227,13 @@ class MaliputViewerModel {
   /// \return Lane associated with that position or nullptr if not found.
   const maliput::api::Lane* GetLaneFromWorldPosition(const ignition::math::Vector3d& _position);
 
+  /// \brief Get the maliput::api::RoadPositionResult associated with a point in
+  ///        world space if exists.
+  /// \pre The RoadGeometry (in case of multilane) or the RoadNetwork must be initialized.
+  /// \param[in] _position World position of point that intersected with a plane
+  /// \return maliput::api::RoadPositionResult associated with that position.
+  const maliput::api::RoadPositionResult GetRoadPositionResult(const ignition::math::Vector3d& _position);
+
   /// \brief Get the lane associated with an id.
   /// \param[in] _id Id of the lane.
   /// \return Lane associated with given id or nullptr if id is invalid.

delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.cc

@@ -566,13 +566,17 @@ ignition::gui::Plugin* MaliputViewerPlugin::FilterPluginsByTitle(const std::stri
 
 bool MaliputViewerPlugin::eventFilter(QObject* _obj, QEvent* _event) {
   if (model->IsInitialized()) {
-    if (_event->type() == QEvent::Type::MouseButtonPress) {
-      const QMouseEvent* mouseEvent = static_cast<QMouseEvent*>(_event);
-      if (mouseEvent && mouseEvent->button() == Qt::LeftButton) {
-        MouseClickHandler(mouseEvent);
-      }
+    if (_event->type() == ignition::gui::events::LeftClickToScene::kType) {
+      auto leftClickToScene = static_cast<ignition::gui::events::LeftClickToScene*>(_event);
+      // TODO(https://github.com/ignitionrobotics/ign-gui/issues/209): use distance to camera once
+      //                                                               it is available.
+      MouseClickHandler(leftClickToScene->Point(), camera->WorldPosition().Distance(leftClickToScene->Point()));
     }
     if (_event->type() == ignition::gui::events::Render::kType) {
+      if (roadPositionResultValue.IsDirty()) {
+        UpdateLaneSelectionOnLeftClick();
+        roadPositionResultValue.SetDirty(false);
+      }
       arrow->Update();
       trafficLightManager->Tick();
       if (renderMeshesOption.executeMeshRendering) {
@@ -589,25 +593,36 @@ bool MaliputViewerPlugin::eventFilter(QObject* _obj, QEvent* _event) {
   return QObject::eventFilter(_obj, _event);
 }
 
-void MaliputViewerPlugin::MouseClickHandler(const QMouseEvent* _mouseEvent) {
-  const auto rayQueryResult = ScreenToScene(_mouseEvent->x(), _mouseEvent->y());
-  if (rayQueryResult.distance >= 0) {
-    const maliput::api::Lane* lane = model->GetLaneFromWorldPosition(rayQueryResult.point);
-    if (lane) {
+void MaliputViewerPlugin::MouseClickHandler(const ignition::math::Vector3d& _sceneInertialPosition, double _distance) {
+  const maliput::api::RoadPositionResult newRoadPositionResult = model->GetRoadPositionResult(_sceneInertialPosition);
+  // There is no intersection vs There is an intersection and should update the the properties of the lane, etc.
+  roadPositionResultValue = newRoadPositionResult.distance > 1e-6
+                                ? RoadPositionResultValue()
+                                : RoadPositionResultValue(newRoadPositionResult, _distance);
+  roadPositionResultValue.SetDirty(true);
+}
+
+void MaliputViewerPlugin::UpdateLaneSelectionOnLeftClick() {
+  if (roadPositionResultValue.Value().has_value()) {
+    if (roadPositionResultValue.Value()->road_position.lane) {
+      const maliput::api::Lane* lane = roadPositionResultValue.Value()->road_position.lane;
       const std::string lane_id = lane->id().string();
       ignmsg << "Clicked lane ID: " << lane_id << std::endl;
       selector->SelectLane(lane);
       // Update visualization to default if it is deselected
       UpdateLane(lane_id);
       UpdateRulesList(lane_id);
-      UpdateLaneInfoArea(rayQueryResult.point);
+      UpdateLaneInfoArea(roadPositionResultValue.Value().value());
 
       const std::string start_bp_id = lane->GetBranchPoint(maliput::api::LaneEnd::kStart)->id().string();
       const std::string end_bp_id = lane->GetBranchPoint(maliput::api::LaneEnd::kFinish)->id().string();
       UpdateBranchPoint(start_bp_id);
       UpdateBranchPoint(end_bp_id);
 
-      arrow->SelectAt(rayQueryResult.distance, rayQueryResult.point);
+      const ignition::math::Vector3d sceneClickPosition(roadPositionResultValue.Value()->nearest_position.x(),
+                                                        roadPositionResultValue.Value()->nearest_position.y(),
+                                                        roadPositionResultValue.Value()->nearest_position.z());
+      arrow->SelectAt(roadPositionResultValue.Distance(), sceneClickPosition);
       arrow->SetVisibility(true);
 
       // Update selected table's lane id.
@@ -625,42 +640,35 @@ void MaliputViewerPlugin::MouseClickHandler(const QMouseEvent* _mouseEvent) {
   }
 }
 
-void MaliputViewerPlugin::UpdateLaneInfoArea(const ignition::math::Vector3d& _pos) {
-  const maliput::api::Lane* lane = model->GetLaneFromWorldPosition(_pos);
-  if (!lane) {
-    return;
-  }
-  const auto lane_pos = lane->ToLanePosition({_pos[0], _pos[1], _pos[2]});
+void MaliputViewerPlugin::UpdateLaneInfoArea(const maliput::api::RoadPositionResult& _roadPositionResult) {
+  const maliput::api::Lane* lane = _roadPositionResult.road_position.lane;
+  const maliput::api::LanePosition& lanePos = _roadPositionResult.road_position.pos;
+  const maliput::api::HBounds hBounds = lane->elevation_bounds(lanePos.s(), lanePos.r());
+  const maliput::api::RBounds startLaneBounds = lane->lane_bounds(0.);
+  const maliput::api::RBounds midLaneBounds = lane->lane_bounds(lane->length() / 2.);
+  const maliput::api::RBounds endLaneBounds = lane->lane_bounds(lane->length());
+  const maliput::api::RBounds laneBounds = lane->lane_bounds(lanePos.s());
   // Update message to be displayed in the info area.
   std::stringstream ss;
   ss << "----  LANE ID: " << lane->id() << "  -----";
   ss << "\nLength ------------> " << lane->length() << "m";
-  ss << "\nLanePosition ------> " << lane_pos.lane_position;
-  ss << "\nInertialPosition --> "
-     << "(x = " << _pos[0] << ", y = " << _pos[1] << ", z = " << _pos[2] << ")";
-  ss << "\nRBounds ------> "
-     << "(min: " << lane->lane_bounds(lane_pos.lane_position.s()).min()
-     << ", max: " << lane->lane_bounds(lane_pos.lane_position.s()).max() << ")";
-  ss << "\nHBounds ------> "
-     << "(min: " << lane->elevation_bounds(lane_pos.lane_position.s(), lane_pos.lane_position.r()).min()
-     << ", max: " << lane->elevation_bounds(lane_pos.lane_position.s(), lane_pos.lane_position.r()).max() << ")";
+  ss << "\nLanePosition ------> " << lanePos;
+  ss << "\nInertialPosition --> " << _roadPositionResult.nearest_position;
+  ss << "\nRBounds ------> (min: " << laneBounds.min() << ", max: " << laneBounds.max() << ")";
+  ss << "\nHBounds ------> (min: " << hBounds.min() << ", max: " << hBounds.max() << ")";
   ss << "\nSegmentId: ------------> " << lane->segment()->id().string();
   ss << "\nJunctionId: ------------> " << lane->segment()->junction()->id().string();
   ss << "\n----  LANE BOUNDARIES (INERTIAL FRAME)  ----";
   ss << "\n(s, r, h) ------> (x, y, z)";
   ss << "\n(0, 0, 0) ----------> " << lane->ToInertialPosition({0., 0., 0.});
-  ss << "\n(0, r_min, 0) ------> " << lane->ToInertialPosition({0., lane->lane_bounds(0.).min(), 0.});
-  ss << "\n(0, r_max, 0) ------> " << lane->ToInertialPosition({0., lane->lane_bounds(0.).max(), 0.});
+  ss << "\n(0, r_min, 0) ------> " << lane->ToInertialPosition({0., startLaneBounds.min(), 0.});
+  ss << "\n(0, r_max, 0) ------> " << lane->ToInertialPosition({0., startLaneBounds.max(), 0.});
   ss << "\n(s_max / 2, 0, 0) ----------> " << lane->ToInertialPosition({lane->length() / 2., 0., 0.});
-  ss << "\n(s_max / 2, r_min, 0) ------> "
-     << lane->ToInertialPosition({lane->length() / 2., lane->lane_bounds(lane->length() / 2.).min(), 0.});
-  ss << "\n(s_max / 2, r_max, 0) ------> "
-     << lane->ToInertialPosition({lane->length() / 2., lane->lane_bounds(lane->length() / 2.).max(), 0.});
+  ss << "\n(s_max / 2, r_min, 0) ------> " << lane->ToInertialPosition({lane->length() / 2., midLaneBounds.min(), 0.});
+  ss << "\n(s_max / 2, r_max, 0) ------> " << lane->ToInertialPosition({lane->length() / 2., midLaneBounds.max(), 0.});
   ss << "\n(s_max, 0, 0) ----------> " << lane->ToInertialPosition({lane->length(), 0., 0.});
-  ss << "\n(s_max, r_min, 0) ------> "
-     << lane->ToInertialPosition({lane->length(), lane->lane_bounds(lane->length()).min(), 0.});
-  ss << "\n(s_max, r_max, 0) ------> "
-     << lane->ToInertialPosition({lane->length(), lane->lane_bounds(lane->length()).max(), 0.});
+  ss << "\n(s_max, r_min, 0) ------> " << lane->ToInertialPosition({lane->length(), endLaneBounds.min(), 0.});
+  ss << "\n(s_max, r_max, 0) ------> " << lane->ToInertialPosition({lane->length(), endLaneBounds.max(), 0.});
 
   laneInfo = QString::fromStdString(ss.str());
   emit LaneInfoChanged();

delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.hh

@@ -3,6 +3,7 @@
 
 #include <map>
 #include <memory>
+#include <optional>
 #include <string>
 #include <unordered_map>
 
@@ -200,6 +201,44 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   /// @brief The rendering engine name.
   static constexpr char const* kEngineName = "ogre";
 
+  /// \brief Holds a maliput::api::RoadPositionResult which results from the
+  ///        mapped scene click position into the Inertial frame and the
+  ///        distance to the camera.
+  /// \details Note that this wrapper may hold a std::nullopt and the dirty flag
+  ///          is true. It helps to change the selection state of the lanes.
+  class RoadPositionResultValue {
+   public:
+    /// \brief Default constructor.
+    RoadPositionResultValue() = default;
+
+    /// \brief Constructs a RoadPositionResultValue from @p _other
+    ///        maliput::api::RoadPositionResult and @p _distance to the camera
+    ///        position in the scene.
+    /// \param _other A maliput::api::RoadPositionResult.
+    /// \param _distance The distance between the camera and the intersected
+    ///        point in the scene.
+    explicit RoadPositionResultValue(const maliput::api::RoadPositionResult& _other, double _distance)
+        : value(_other), distance(_distance), dirty(true) {}
+
+    /// \brief Sets the dirty flag.
+    /// \param _isDirty Whether the value is dirty or not.
+    void SetDirty(bool _isDirty) { dirty = _isDirty; }
+
+    /// \return The distance between the clicked scene position and the camera.
+    double Distance() const { return distance; }
+
+    /// \return True when the value is dirty.
+    bool IsDirty() const { return dirty; }
+
+    /// \return The stored optional with maliput::api::RoadPositionResult.
+    const std::optional<maliput::api::RoadPositionResult>& Value() const { return value; }
+
+   private:
+    std::optional<maliput::api::RoadPositionResult> value{std::nullopt};
+    double distance{0.};
+    bool dirty{false};
+  };
+
   /// \brief Fills a material for a lane label.
   /// \param[in] _material Material to be filled.
   static void CreateLaneLabelMaterial(ignition::rendering::MaterialPtr& _material);
@@ -246,9 +285,14 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   ///          is expected to be titled as #kMainScene3dPlugin.
   void Initialize();
 
-  /// \brief Handles the left click mouse event.
-  /// @param[in] _mouseEvent QMouseEvent pointer.
-  void MouseClickHandler(const QMouseEvent* _mouseEvent);
+  /// \brief Handles the left click mouse event and populates roadPositionResultValue.
+  /// \param[in] _sceneInertialPosition The scene (Inertial) frame position to
+  ///            map into the RoadGeometry.
+  /// \param[in] _distance The distance between the camera and @p _sceneInertialPosition.
+  void MouseClickHandler(const ignition::math::Vector3d& _sceneInertialPosition, double _distance);
+
+  /// \brief Handles the left click mouse event and populates roadPositionResultValue.
+  void UpdateLaneSelectionOnLeftClick();
 
   /// \brief Performs a raycast on the screen.
   /// \param[in] screenX X screen's coordinate.
@@ -286,8 +330,8 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
   void UpdateRulesList(const std::string& _laneId);
 
   /// \brief Updates the text related to the clicked surface if any.
-  /// \param[in] _pos Coordinate in the Inertial Frame.
-  void UpdateLaneInfoArea(const ignition::math::Vector3d& _pos);
+  /// \param[in] _roadPositionResult Result of the mapped RoadPosition after a click in the scene.
+  void UpdateLaneInfoArea(const maliput::api::RoadPositionResult& _roadPositionResult);
 
   /// Keys used by the checkbox logic to visualize different layers and by
   /// the default map #objectVisualDefaults.
@@ -378,6 +422,9 @@ class MaliputViewerPlugin : public ignition::gui::Plugin {
 
   /// \brief Manager of traffic lights visualization.
   std::unique_ptr<TrafficLightManager> trafficLightManager;
+
+  /// \brief Holds the RoadPositionResult upon a left click in the scene.
+  RoadPositionResultValue roadPositionResultValue;
 };
 
 }  // namespace gui