Skip to content

Commit

Permalink
tweaks
Browse files Browse the repository at this point in the history
Signed-off-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
chapulina committed Sep 8, 2021
1 parent 9850354 commit d93a125
Show file tree
Hide file tree
Showing 4 changed files with 71 additions and 77 deletions.
3 changes: 2 additions & 1 deletion examples/worlds/minimal_scene.sdf
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,15 @@ Features:
* Grid config
* Select entities
* Transform controls
* Spawn entities through GUI
Missing for parity with GzScene3D:
* Spawn entities through GUI
* Context menu
* Record video
* View angles
* View collisions, wireframe, transparent, CoM, etc
* Drag and drop from Fuel / meshes
* ...
-->
Expand Down
16 changes: 8 additions & 8 deletions src/gui/plugins/select_entities/SelectEntities.cc
Original file line number Diff line number Diff line change
Expand Up @@ -136,8 +136,8 @@ class ignition::gazebo::gui::SelectEntitiesPrivate
/// \brief is transform control active ?
public: bool transformControlActive = false;

/// \brief is Spawning from description active
public: bool isSpawnFromDescription{false};
/// \brief Is an entity being spawned
public: bool isSpawning{false};
};

using namespace ignition;
Expand Down Expand Up @@ -488,13 +488,13 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event)
ignition::gui::events::LeftClickOnScene *_e =
static_cast<ignition::gui::events::LeftClickOnScene*>(_event);
this->dataPtr->mouseEvent = _e->Mouse();
// handle transform control

if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT &&
this->dataPtr->mouseEvent.Type() == common::MouseEvent::PRESS)
this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE)
{
if(this->dataPtr->isSpawnFromDescription)
if (this->dataPtr->isSpawning)
{
this->dataPtr->isSpawnFromDescription = false;
this->dataPtr->isSpawning = false;
}
else
{
Expand Down Expand Up @@ -559,7 +559,7 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event)
ignition::gui::events::SpawnFromDescription::kType ||
_event->type() == ignition::gui::events::SpawnFromPath::kType)
{
this->dataPtr->isSpawnFromDescription = true;
this->dataPtr->isSpawning = true;
this->dataPtr->mouseDirty = true;
}
else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType)
Expand All @@ -570,7 +570,7 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event)
{
this->dataPtr->mouseDirty = true;
this->dataPtr->selectionHelper.deselectAll = true;
this->dataPtr->isSpawnFromDescription = false;
this->dataPtr->isSpawning = false;
}
}

Expand Down
121 changes: 59 additions & 62 deletions src/gui/plugins/spawn/Spawn.cc
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@
#include "Spawn.hh"

#include <ignition/msgs/boolean.pb.h>
#include <ignition/msgs/stringmsg.pb.h>
#include <ignition/msgs/entity_factory.pb.h>

#include <algorithm>
#include <limits>
Expand All @@ -36,6 +36,7 @@
#include <ignition/gui/MainWindow.hh>

#include <ignition/math/Vector2.hh>
#include <ignition/msgs/Utility.hh>

#include <ignition/plugin/Register.hh>

Expand All @@ -57,7 +58,7 @@ namespace ignition::gazebo
{
class SpawnPrivate
{
/// \brief Update the 3D scene with new entities
/// \brief Perform operations in the render thread.
public: void OnRender();

/// \brief Delete the visuals generated while an entity is being spawned.
Expand All @@ -68,8 +69,8 @@ namespace ignition::gazebo
/// \return True on success, false if failure
public: bool GeneratePreview(const sdf::Root &_sdf);

/// \brief Handle model placement requests
public: void HandleModelPlacement();
/// \brief Handle placement requests
public: void HandlePlacement();

/// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a
/// ray cast from the given 2D screen coordinates.
Expand All @@ -79,23 +80,23 @@ namespace ignition::gazebo
/// \param[in] _offset Offset along the plane normal
/// \return 3D coordinates of a point in the 3D scene.
math::Vector3d ScreenToPlane(
const ignition::math::Vector2i &_screenPos,
const ignition::rendering::CameraPtr &_camera,
const ignition::rendering::RayQueryPtr &_rayQuery,
const math::Vector2i &_screenPos,
const rendering::CameraPtr &_camera,
const rendering::RayQueryPtr &_rayQuery,
const float offset = 0.0);

/// \brief Generate a unique entity id.
/// \return The unique entity id
ignition::gazebo::Entity UniqueId();
Entity UniqueId();

/// \brief Ignition communication node.
public: transport::Node node;

/// \brief Flag for indicating whether we are spawning or not.
public: bool isSpawning = false;
/// \brief Flag for indicating whether the preview needs to be generated.
public: bool generatePreview = false;

/// \brief Flag for indicating whether the user is currently placing a
/// resource with the shapes plugin or not
/// resource or not
public: bool isPlacing = false;

/// \brief The SDF string of the resource to be used with plugins that spawn
Expand All @@ -106,21 +107,21 @@ namespace ignition::gazebo
public: std::string spawnSdfPath;

/// \brief Pointer to the rendering scene
public: ignition::rendering::ScenePtr scene{nullptr};
public: rendering::ScenePtr scene{nullptr};

/// \brief A record of the ids currently used by the entity spawner
/// for easy deletion of visuals later
public: std::vector<ignition::gazebo::Entity> previewIds;
public: std::vector<Entity> previewIds;

/// \brief The visual generated from the spawnSdfString
public: ignition::rendering::NodePtr spawnPreview{nullptr};
/// \brief Pointer to the preview that the user is placing.
public: rendering::NodePtr spawnPreview{nullptr};

/// \brief Scene manager
public: ignition::gazebo::SceneManager sceneManager;
public: SceneManager sceneManager;

/// \brief The pose of the spawn preview.
public: ignition::math::Pose3d spawnPreviewPose =
ignition::math::Pose3d::Zero;
public: math::Pose3d spawnPreviewPose =
math::Pose3d::Zero;

/// \brief Mouse event
public: common::MouseEvent mouseEvent;
Expand Down Expand Up @@ -180,7 +181,7 @@ void Spawn::LoadConfig(const tinyxml2::XMLElement *)
}


// TODO(ahcorde): Replace this when this PR is on ign-rendering6
// TODO(ahcorde): Replace this when this function is on ign-rendering6
/////////////////////////////////////////////////
math::Vector3d SpawnPrivate::ScreenToPlane(
const math::Vector2i &_screenPos,
Expand All @@ -199,7 +200,7 @@ math::Vector3d SpawnPrivate::ScreenToPlane(
_rayQuery->SetFromCamera(
_camera, math::Vector2d(nx, ny));

ignition::math::Planed plane(ignition::math::Vector3d(0, 0, 1), offset);
math::Planed plane(math::Vector3d(0, 0, 1), offset);

math::Vector3d origin = _rayQuery->Origin();
math::Vector3d direction = _rayQuery->Direction();
Expand All @@ -208,7 +209,7 @@ math::Vector3d SpawnPrivate::ScreenToPlane(
}

/////////////////////////////////////////////////
void SpawnPrivate::HandleModelPlacement()
void SpawnPrivate::HandlePlacement()
{
if (!this->isPlacing)
return;
Expand All @@ -228,16 +229,16 @@ void SpawnPrivate::HandleModelPlacement()
// Delete the generated visuals
this->TerminateSpawnPreview();

math::Pose3d modelPose = this->spawnPreviewPose;
std::function<void(const ignition::msgs::Boolean &, const bool)> cb =
[](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
auto pose = this->spawnPreviewPose;
std::function<void(const msgs::Boolean &, const bool)> cb =
[](const msgs::Boolean &/*_rep*/, const bool _result)
{
if (!_result)
ignerr << "Error creating model" << std::endl;
ignerr << "Error creating entity" << std::endl;
};
math::Vector3d pos = this->ScreenToPlane(
this->mouseEvent.Pos(), this->camera, this->rayQuery);
pos.Z(modelPose.Pos().Z());
pos.Z(pose.Pos().Z());
msgs::EntityFactory req;
if (!this->spawnSdfString.empty())
{
Expand All @@ -252,7 +253,7 @@ void SpawnPrivate::HandleModelPlacement()
ignwarn << "Failed to find SDF string or file path" << std::endl;
}
req.set_allow_renaming(true);
msgs::Set(req.mutable_pose(), math::Pose3d(pos, modelPose.Rot()));
msgs::Set(req.mutable_pose(), math::Pose3d(pos, pose.Rot()));

if (this->createCmdService.empty())
{
Expand All @@ -277,7 +278,7 @@ void SpawnPrivate::HandleModelPlacement()
}

/////////////////////////////////////////////////
ignition::gazebo::Entity SpawnPrivate::UniqueId()
Entity SpawnPrivate::UniqueId()
{
auto timeout = 100000u;
for (auto i = 0u; i < timeout; ++i)
Expand Down Expand Up @@ -324,7 +325,7 @@ void SpawnPrivate::OnRender()

// Spawn
IGN_PROFILE("IgnRenderer::Render Spawn");
if (this->isSpawning)
if (this->generatePreview)
{
// Generate spawn preview
rendering::VisualPtr rootVis = this->scene->RootVisual();
Expand All @@ -342,7 +343,7 @@ void SpawnPrivate::OnRender()
ignwarn << "Failed to spawn: no SDF string or path" << std::endl;
}
this->isPlacing = this->GeneratePreview(root);
this->isSpawning = false;
this->generatePreview = false;
}

// Escape action, clear all selections and terminate any
Expand All @@ -355,14 +356,16 @@ void SpawnPrivate::OnRender()
}
}

this->HandleModelPlacement();
this->HandlePlacement();
}

/////////////////////////////////////////////////
void SpawnPrivate::TerminateSpawnPreview()
{
for (auto _id : this->previewIds)
{
this->sceneManager.RemoveEntity(_id);
}
this->previewIds.clear();
this->isPlacing = false;
}
Expand All @@ -375,8 +378,8 @@ bool SpawnPrivate::GeneratePreview(const sdf::Root &_sdf)

if (nullptr == _sdf.Model() && nullptr == _sdf.Light())
{
ignwarn << "Only model entities can be spawned at the moment." << std::endl;
this->TerminateSpawnPreview();
ignwarn << "Only model or light entities can be spawned at the moment."
<< std::endl;
return false;
}

Expand All @@ -385,54 +388,50 @@ bool SpawnPrivate::GeneratePreview(const sdf::Root &_sdf)
// Only preview first model
sdf::Model model = *(_sdf.Model());
this->spawnPreviewPose = model.RawPose();
model.SetName(ignition::common::Uuid().String());
model.SetName(common::Uuid().String());
Entity modelId = this->UniqueId();
if (!modelId)
if (kNullEntity == modelId)
{
this->TerminateSpawnPreview();
return false;
}
this->spawnPreview =
this->sceneManager.CreateModel(
modelId, model,
this->sceneManager.WorldId());
this->spawnPreview = this->sceneManager.CreateModel(
modelId, model, this->sceneManager.WorldId());

this->previewIds.push_back(modelId);
for (auto j = 0u; j < model.LinkCount(); j++)
{
sdf::Link link = *(model.LinkByIndex(j));
link.SetName(ignition::common::Uuid().String());
link.SetName(common::Uuid().String());
Entity linkId = this->UniqueId();
if (!linkId)
{
this->TerminateSpawnPreview();
return false;
}
this->sceneManager.CreateLink(
linkId, link, modelId);
this->sceneManager.CreateLink(linkId, link, modelId);
this->previewIds.push_back(linkId);
for (auto k = 0u; k < link.VisualCount(); k++)
{
sdf::Visual visual = *(link.VisualByIndex(k));
visual.SetName(ignition::common::Uuid().String());
Entity visualId = this->UniqueId();
if (!visualId)
{
this->TerminateSpawnPreview();
return false;
}
this->sceneManager.CreateVisual(
visualId, visual, linkId);
this->previewIds.push_back(visualId);
sdf::Visual visual = *(link.VisualByIndex(k));
visual.SetName(common::Uuid().String());
Entity visualId = this->UniqueId();
if (!visualId)
{
this->TerminateSpawnPreview();
return false;
}
this->sceneManager.CreateVisual(visualId, visual, linkId);
this->previewIds.push_back(visualId);
}
}
}
else if (_sdf.Light())
{
// Only preview first model
// Only preview first light
sdf::Light light = *(_sdf.Light());
this->spawnPreviewPose = light.RawPose();
light.SetName(ignition::common::Uuid().String());
light.SetName(common::Uuid().String());
Entity lightVisualId = this->UniqueId();
if (!lightVisualId)
{
Expand All @@ -445,10 +444,8 @@ bool SpawnPrivate::GeneratePreview(const sdf::Root &_sdf)
this->TerminateSpawnPreview();
return false;
}
this->spawnPreview =
this->sceneManager.CreateLight(
lightId, light,
this->sceneManager.WorldId());
this->spawnPreview = this->sceneManager.CreateLight(
lightId, light, this->sceneManager.WorldId());
this->sceneManager.CreateLightVisual(
lightVisualId, light, lightId);

Expand All @@ -470,7 +467,7 @@ bool Spawn::eventFilter(QObject *_obj, QEvent *_event)
ignition::gui::events::LeftClickOnScene *_e =
static_cast<ignition::gui::events::LeftClickOnScene*>(_event);
this->dataPtr->mouseEvent = _e->Mouse();
if (this->dataPtr->isSpawning || this->dataPtr->isPlacing)
if (this->dataPtr->generatePreview || this->dataPtr->isPlacing)
this->dataPtr->mouseDirty = true;
}
else if (_event->type() == ignition::gui::events::HoverOnScene::kType)
Expand All @@ -486,14 +483,14 @@ bool Spawn::eventFilter(QObject *_obj, QEvent *_event)
ignition::gui::events::SpawnFromDescription *_e =
static_cast<ignition::gui::events::SpawnFromDescription*>(_event);
this->dataPtr->spawnSdfString = _e->Description();
this->dataPtr->isSpawning = true;
this->dataPtr->generatePreview = true;
}
else if (_event->type() == ignition::gui::events::SpawnFromPath::kType)
{
auto spawnPreviewPathEvent =
reinterpret_cast<ignition::gui::events::SpawnFromPath *>(_event);
this->dataPtr->spawnSdfPath = spawnPreviewPathEvent->FilePath();
this->dataPtr->isSpawning = true;
this->dataPtr->generatePreview = true;
}
else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType)
{
Expand Down
Loading

0 comments on commit d93a125

Please sign in to comment.