Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Tape Measure Plugin #456

Merged
merged 29 commits into from
Dec 16, 2020
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
29 commits
Select commit Hold shift + click to select a range
7d4ab56
init plugin template
Oct 22, 2020
32236cc
some updates
Oct 28, 2020
a062e20
Add marker placement
Nov 10, 2020
af399c9
Complete functionality
Nov 10, 2020
3f7f265
Tidy up
Nov 11, 2020
8f107d6
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 11, 2020
041e54b
Update docs
Nov 11, 2020
22d1c0f
Clean up
Nov 11, 2020
f0b022a
revert some changes
Nov 11, 2020
0dfec2f
Add crosshair cursor
Nov 11, 2020
38a6e4e
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 11, 2020
a0f20f6
Add shortcut and new icon
Nov 11, 2020
cf77ad8
Merge branch 'jshep1/tape_measure_plugin' of https://github.com/ignit…
Nov 11, 2020
83b0a43
Updates
Nov 12, 2020
e69d95a
Add in trashcan icon
Nov 13, 2020
161e979
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 16, 2020
59d5b33
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 17, 2020
df26ea2
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 20, 2020
a7cbc13
add in gui events
Nov 20, 2020
a31264c
Remove events from gazebo gui
Nov 20, 2020
dda9038
requested fixes
Nov 20, 2020
4d41ebe
More fixes
Nov 20, 2020
744e2db
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Nov 24, 2020
65d122a
handle key presses on cpp side
Nov 24, 2020
acaac9c
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Dec 7, 2020
d8a7404
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Dec 10, 2020
762a2d5
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Dec 14, 2020
aecce0c
update required gui version
Dec 15, 2020
2f47f18
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin
Dec 16, 2020
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -75,7 +75,7 @@ set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools4_VERSION_MAJOR})

#--------------------------------------
# Find ignition-gui
ign_find_package(ignition-gui3 REQUIRED VERSION 3.1)
ign_find_package(ignition-gui3 REQUIRED VERSION 3.4)
set(IGN_GUI_VER ${ignition-gui3_VERSION_MAJOR})
ign_find_package (Qt5
COMPONENTS
Expand Down
1 change: 1 addition & 0 deletions src/gui/plugins/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -89,6 +89,7 @@ add_subdirectory(playback_scrubber)
add_subdirectory(resource_spawner)
add_subdirectory(scene3d)
add_subdirectory(shapes)
add_subdirectory(tape_measure)
add_subdirectory(transform_control)
add_subdirectory(video_recorder)
add_subdirectory(view_angle)
33 changes: 33 additions & 0 deletions src/gui/plugins/scene3d/Scene3D.cc
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,7 @@
#include <ignition/transport/Node.hh>

#include <ignition/gui/Conversions.hh>
#include <ignition/gui/GuiEvents.hh>
#include <ignition/gui/Application.hh>
#include <ignition/gui/MainWindow.hh>

Expand Down Expand Up @@ -767,12 +768,44 @@ Entity IgnRenderer::UniqueId()
void IgnRenderer::HandleMouseEvent()
{
std::lock_guard<std::mutex> lock(this->dataPtr->mutex);
this->BroadcastHoverPos();
this->BroadcastLeftClick();
this->HandleMouseContextMenu();
this->HandleModelPlacement();
this->HandleMouseTransformControl();
this->HandleMouseViewControl();
}

/////////////////////////////////////////////////
void IgnRenderer::BroadcastHoverPos()
{
if (this->dataPtr->hoverDirty)
{
math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseHoverPos);
chapulina marked this conversation as resolved.
Show resolved Hide resolved

ignition::gui::events::HoverToScene hoverToSceneEvent(pos);
ignition::gui::App()->sendEvent(
ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
&hoverToSceneEvent);
}
}

/////////////////////////////////////////////////
void IgnRenderer::BroadcastLeftClick()
{
if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT &&
this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE &&
!this->dataPtr->mouseEvent.Dragging() && this->dataPtr->mouseDirty)
{
math::Vector3d pos = this->ScreenToScene(this->dataPtr->mouseEvent.Pos());

ignition::gui::events::LeftClickToScene leftClickToSceneEvent(pos);
ignition::gui::App()->sendEvent(
ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
&leftClickToSceneEvent);
}
}

/////////////////////////////////////////////////
void IgnRenderer::HandleMouseContextMenu()
{
Expand Down
6 changes: 6 additions & 0 deletions src/gui/plugins/scene3d/Scene3D.hh
Original file line number Diff line number Diff line change
Expand Up @@ -351,6 +351,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE {
/// \brief Handle model placement requests
private: void HandleModelPlacement();

/// \brief Broadcasts the currently hovered 3d scene location.
private: void BroadcastHoverPos();

/// \brief Broadcasts a left click within the scene
private: void BroadcastLeftClick();

/// \brief Generate a unique entity id.
/// \return The unique entity id
private: Entity UniqueId();
Expand Down
6 changes: 6 additions & 0 deletions src/gui/plugins/tape_measure/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
gz_add_gui_plugin(TapeMeasure
SOURCES TapeMeasure.cc
QT_HEADERS TapeMeasure.hh
PRIVATE_LINK_LIBS
${IGNITION-RENDERING_LIBRARIES}
)
301 changes: 301 additions & 0 deletions src/gui/plugins/tape_measure/TapeMeasure.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,301 @@
/*
* Copyright (C) 2020 Open Source Robotics Foundation
*
* 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.
*
*/

#include "ignition/gazebo/gui/GuiEvents.hh"

#include "TapeMeasure.hh"

#include <iostream>
#include <unordered_set>
#include <string>
#include <memory>

#include <ignition/common/Console.hh>
#include <ignition/gui/Application.hh>
#include <ignition/gui/GuiEvents.hh>
#include <ignition/gui/MainWindow.hh>
#include <ignition/msgs/marker.pb.h>
#include <ignition/msgs/Utility.hh>
#include <ignition/plugin/Register.hh>
#include <ignition/transport/Node.hh>
#include <ignition/transport/Publisher.hh>

namespace ignition::gazebo
{
class TapeMeasurePrivate
{
/// \brief Ignition communication node.
public: transport::Node node;

/// \brief True if currently measuring, else false.
public: bool measure = false;

/// \brief The id of the start point marker.
public: const int kStartPointId = 1;

/// \brief The id of the end point marker.
public: const int kEndPointId = 2;

/// \brief The id of the line marker.
public: const int kLineId = 3;

/// \brief The id of the start or end point marker that is currently
/// being placed. This is primarily used to track the state machine of
/// the plugin.
public: int currentId = kStartPointId;

/// \brief The location of the placed starting point of the tape measure
/// tool, only set when the user clicks to set the point.
public: ignition::math::Vector3d startPoint =
ignition::math::Vector3d::Zero;

/// \brief The location of the placed ending point of the tape measure
/// tool, only set when the user clicks to set the point.
public: ignition::math::Vector3d endPoint = ignition::math::Vector3d::Zero;

/// \brief The color to set the marker when hovering the mouse over the
/// scene.
public: ignition::math::Color
hoverColor{ignition::math::Color(0.2, 0.2, 0.2, 0.5)};

/// \brief The color to draw the marker when the user clicks to confirm
/// its location.
public: ignition::math::Color
drawColor{ignition::math::Color(0.2, 0.2, 0.2, 1.0)};

/// \brief A set of the currently placed markers. Used to make sure a
/// non-existent marker is not deleted.
public: std::unordered_set<int> placedMarkers;

/// \brief The current distance between the two points. This distance
/// is updated as the user hovers the end point as well.
public: double distance = 0.0;

/// \brief The namespace that the markers for this plugin are placed in.
public: std::string ns = "tape_measure";
};
}

using namespace ignition;
using namespace gazebo;

/////////////////////////////////////////////////
TapeMeasure::TapeMeasure()
: ignition::gui::Plugin(),
dataPtr(std::make_unique<TapeMeasurePrivate>())
{
}

/////////////////////////////////////////////////
TapeMeasure::~TapeMeasure() = default;

/////////////////////////////////////////////////
void TapeMeasure::LoadConfig(const tinyxml2::XMLElement *)
{
if (this->title.empty())
this->title = "Tape measure";

ignition::gui::App()->findChild<ignition::gui::MainWindow *>
()->installEventFilter(this);
ignition::gui::App()->findChild<ignition::gui::MainWindow *>
()->QuickWindow()->installEventFilter(this);
}

/////////////////////////////////////////////////
void TapeMeasure::OnMeasure()
{
this->Measure();
}

/////////////////////////////////////////////////
void TapeMeasure::Measure()
{
this->Reset();
this->dataPtr->measure = true;
QGuiApplication::setOverrideCursor(Qt::CrossCursor);
}

/////////////////////////////////////////////////
void TapeMeasure::OnReset()
{
this->Reset();
}

/////////////////////////////////////////////////
void TapeMeasure::Reset()
{
this->DeleteMarker(this->dataPtr->kStartPointId);
this->DeleteMarker(this->dataPtr->kEndPointId);
this->DeleteMarker(this->dataPtr->kLineId);

this->dataPtr->currentId = this->dataPtr->kStartPointId;
this->dataPtr->startPoint = ignition::math::Vector3d::Zero;
this->dataPtr->endPoint = ignition::math::Vector3d::Zero;
this->dataPtr->distance = 0.0;
this->dataPtr->measure = false;
this->newDistance();
QGuiApplication::restoreOverrideCursor();
}

/////////////////////////////////////////////////
double TapeMeasure::Distance()
{
return this->dataPtr->distance;
}

/////////////////////////////////////////////////
void TapeMeasure::DeleteMarker(int _id)
{
if (this->dataPtr->placedMarkers.find(_id) ==
this->dataPtr->placedMarkers.end())
return;

// Delete the previously created marker
ignition::msgs::Marker markerMsg;
markerMsg.set_ns(this->dataPtr->ns);
markerMsg.set_id(_id);
markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER);
this->dataPtr->node.Request("/marker", markerMsg);
this->dataPtr->placedMarkers.erase(_id);
}

/////////////////////////////////////////////////
void TapeMeasure::DrawPoint(int _id,
ignition::math::Vector3d &_point, ignition::math::Color &_color)
{
this->DeleteMarker(_id);

ignition::msgs::Marker markerMsg;
markerMsg.set_ns(this->dataPtr->ns);
markerMsg.set_id(_id);
markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
markerMsg.set_type(ignition::msgs::Marker::SPHERE);
ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color);
ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color);
ignition::msgs::Set(markerMsg.mutable_scale(),
ignition::math::Vector3d(0.1, 0.1, 0.1));
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I know this will probably be complicated to achieve with markers, but I'll just make an observation for the future.

The fixed scale means that the points may be too big or too small depending on what's being simulated and the user's zoom level. Maybe they're too big to measure the distance between fingers of a robot hand, but too small when measuring the size of a road. One option would be to scale it according to the zoom level like it's done for the transform controls:

scale_tape

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Good point, I believe that functionality is coming from this line? After thinking about it for a second, I think the effect comes from the world "moving away" and the visual gizmo staying the same size. @ColeOSRF do you know how this tool is usually handled in large vs small scenarios and when the user is zooming in and out?

ignition::msgs::Set(markerMsg.mutable_pose(),
ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0));

this->dataPtr->node.Request("/marker", markerMsg);
this->dataPtr->placedMarkers.insert(_id);
}

/////////////////////////////////////////////////
void TapeMeasure::DrawLine(int _id, ignition::math::Vector3d &_startPoint,
ignition::math::Vector3d &_endPoint, ignition::math::Color &_color)
{
this->DeleteMarker(_id);

ignition::msgs::Marker markerMsg;
markerMsg.set_ns(this->dataPtr->ns);
markerMsg.set_id(_id);
markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY);
markerMsg.set_type(ignition::msgs::Marker::LINE_LIST);
ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color);
ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color);
ignition::msgs::Set(markerMsg.add_point(), _startPoint);
ignition::msgs::Set(markerMsg.add_point(), _endPoint);

this->dataPtr->node.Request("/marker", markerMsg);
this->dataPtr->placedMarkers.insert(_id);
}

/////////////////////////////////////////////////
bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event)
{
if (_event->type() == ignition::gui::events::HoverToScene::kType)
{
auto hoverToSceneEvent =
reinterpret_cast<ignition::gui::events::HoverToScene *>(_event);

// This event is called in Scene3d's RenderThread, so it's safe to make
// rendering calls here
if (this->dataPtr->measure && hoverToSceneEvent)
{
ignition::math::Vector3d point = hoverToSceneEvent->Point();
this->DrawPoint(this->dataPtr->currentId, point,
this->dataPtr->hoverColor);

// If the user is currently choosing the end point, draw the connecting
// line and update the new distance.
if (this->dataPtr->currentId == this->dataPtr->kEndPointId)
{
this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint,
point, this->dataPtr->hoverColor);
this->dataPtr->distance = this->dataPtr->startPoint.Distance(point);
this->newDistance();
}
}
}
else if (_event->type() == ignition::gui::events::LeftClickToScene::kType)
{
auto leftClickToSceneEvent =
reinterpret_cast<ignition::gui::events::LeftClickToScene *>(_event);

// This event is called in Scene3d's RenderThread, so it's safe to make
// rendering calls here
if (this->dataPtr->measure && leftClickToSceneEvent)
{
ignition::math::Vector3d point = leftClickToSceneEvent->Point();
this->DrawPoint(this->dataPtr->currentId, point,
this->dataPtr->drawColor);
// If the user is placing the start point, update its position
if (this->dataPtr->currentId == this->dataPtr->kStartPointId)
{
this->dataPtr->startPoint = point;
}
// If the user is placing the end point, update the end position,
// end the measurement state, and update the draw line and distance
else
{
this->dataPtr->endPoint = point;
this->dataPtr->measure = false;
this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint,
this->dataPtr->endPoint, this->dataPtr->drawColor);
this->dataPtr->distance =
this->dataPtr->startPoint.Distance(this->dataPtr->endPoint);
this->newDistance();
QGuiApplication::restoreOverrideCursor();
}
this->dataPtr->currentId = this->dataPtr->kEndPointId;
}
}
else if (_event->type() == QEvent::KeyPress)
{
QKeyEvent *keyEvent = static_cast<QKeyEvent*>(_event);
if (keyEvent && keyEvent->key() == Qt::Key_M)
{
this->Reset();
this->Measure();
}
}
else if (_event->type() == QEvent::KeyRelease)
{
QKeyEvent *keyEvent = static_cast<QKeyEvent*>(_event);
if (keyEvent && keyEvent->key() == Qt::Key_Escape &&
this->dataPtr->measure)
{
this->Reset();
}
}
return QObject::eventFilter(_obj, _event);
}

// Register this plugin
IGNITION_ADD_PLUGIN(ignition::gazebo::TapeMeasure,
ignition::gui::Plugin)
Loading