diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.cc b/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.cc index ef439393..99b77f43 100644 --- a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.cc +++ b/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 = diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.hh b/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.hh index 290dfd44..cda2f19c 100644 --- a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_model.hh +++ b/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. diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.cc b/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.cc index b058b4ab..cd720ab3 100644 --- a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.cc +++ b/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(_event); - if (mouseEvent && mouseEvent->button() == Qt::LeftButton) { - MouseClickHandler(mouseEvent); - } + if (_event->type() == ignition::gui::events::LeftClickToScene::kType) { + auto leftClickToScene = static_cast(_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(); diff --git a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.hh b/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.hh index f2088d7f..4b6bcb5e 100644 --- a/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.hh +++ b/delphyne_gui/visualizer/maliput_viewer_plugin/maliput_viewer_plugin.hh @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -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& Value() const { return value; } + + private: + std::optional 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; + + /// \brief Holds the RoadPositionResult upon a left click in the scene. + RoadPositionResultValue roadPositionResultValue; }; } // namespace gui