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

Disable right click menu when using measuring tool #458

Merged
merged 35 commits into from
Dec 29, 2020
Merged
Changes from 1 commit
Commits
Show all changes
35 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
2a84ddd
disable right menu when measuring
Nov 11, 2020
b07c5d9
Add docs
Nov 12, 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
521939a
Merge branch 'jshep1/tape_measure_plugin' into jshep1/tape_measure_pl…
Dec 10, 2020
a07db06
Update for new gui events
Dec 11, 2020
0dd5811
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin_pt2
Dec 16, 2020
8be928a
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin_pt2
chapulina Dec 18, 2020
de63c5b
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin_pt2
Dec 18, 2020
2957f5c
Merge branch 'ign-gazebo3' into jshep1/tape_measure_plugin_pt2
Dec 28, 2020
255a162
suggested fixes and lint
Dec 28, 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
Prev Previous commit
Next Next commit
Tidy up
Signed-off-by: John Shepherd <[email protected]>
John Shepherd committed Nov 11, 2020
commit 3f7f26532465e57c34e21bbf60df03c6b1e9c77c
137 changes: 84 additions & 53 deletions src/gui/plugins/tape_measure/TapeMeasure.cc
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2019 Open Source Robotics Foundation
* 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.
@@ -47,30 +47,55 @@ namespace ignition::gazebo
/// \brief Ignition communication node.
public: transport::Node node;

/// \brief Mutex to protect mode
/// \brief Mutex to protect mode.
public: std::mutex mutex;

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

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

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

public: ignition::math::Vector3d startPoint = ignition::math::Vector3d::Zero;
public: ignition::math::Vector3d endPoint = ignition::math::Vector3d::Zero;
/// \brief The id of the line marker.
public: int lineId = 3;

public: ignition::math::Vector4d hoverColor = ignition::math::Vector4d(0.2, 0.2, 0.2, 0.5);
public: ignition::math::Vector4d drawColor = ignition::math::Vector4d(0.2, 0.2, 0.2, 1.0);
/// \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 = startPointId;

public: std::unordered_set<int> placedMarkers;
/// \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;

public: int lineId = 3;
/// \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;

public: int currentId = startPointId;
/// \brief The color to set the marker when hovering the mouse over the
/// scene.
public: ignition::math::Vector4d hoverColor =
ignition::math::Vector4d(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::Vector4d drawColor =
ignition::math::Vector4d(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;

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

@@ -110,52 +135,59 @@ void TapeMeasure::OnReset()
this->Reset();
}

/////////////////////////////////////////////////
void TapeMeasure::Reset()
{
this->DeleteMarker(this->dataPtr->startPointId);
this->DeleteMarker(this->dataPtr->endPointId);
this->DeleteMarker(this->dataPtr->lineId);

this->dataPtr->currentId = this->dataPtr->startPointId;
this->dataPtr->startPoint = ignition::math::Vector3d::Zero;
this->dataPtr->endPoint = ignition::math::Vector3d::Zero;
this->dataPtr->placedMarkers.clear();
this->dataPtr->distance = 0.0;
this->newDistance();

if (this->dataPtr->placed)
{
// Delete the previously created marker
ignition::msgs::Marker markerMsg;
markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL);
this->dataPtr->node.Request("/marker", markerMsg);
this->dataPtr->placed = false;
}
}

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

void TapeMeasure::DrawPoint(int id, math::Vector3d &_point, math::Vector4d &_color)
/////////////////////////////////////////////////
void TapeMeasure::DeleteMarker(int _id)
{
ignition::msgs::Marker markerMsg;
if (this->dataPtr->placedMarkers.find(id) != this->dataPtr->placedMarkers.end())
if (this->dataPtr->placedMarkers.find(_id) !=
this->dataPtr->placedMarkers.end())
{
// Delete the previously created marker
markerMsg.set_ns("default");
markerMsg.set_id(id);
markerMsg.set_ns(this->dataPtr->ns);
markerMsg.set_id(_id);
markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER);
this->dataPtr->node.Request("/marker", markerMsg);
}
else
{
this->dataPtr->placedMarkers.insert(id);
this->dataPtr->placedMarkers.insert(_id);
}
}

markerMsg.set_ns("default");
markerMsg.set_id(id);
/////////////////////////////////////////////////
void TapeMeasure::DrawPoint(int _id,
math::Vector3d &_point, math::Vector4d &_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_scale(),
ignition::math::Vector3d(0.1, 0.1, 0.1));
ignition::math::Vector3d(0.1, 0.1, 0.1));
markerMsg.mutable_material()->mutable_ambient()->set_r(_color[0]);
markerMsg.mutable_material()->mutable_ambient()->set_g(_color[1]);
markerMsg.mutable_material()->mutable_ambient()->set_b(_color[2]);
@@ -165,28 +197,19 @@ void TapeMeasure::DrawPoint(int id, math::Vector3d &_point, math::Vector4d &_col
markerMsg.mutable_material()->mutable_diffuse()->set_b(_color[2]);
markerMsg.mutable_material()->mutable_diffuse()->set_a(_color[3]);
ignition::msgs::Set(markerMsg.mutable_pose(),
ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0));
ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0));
this->dataPtr->node.Request("/marker", markerMsg);
}

void TapeMeasure::DrawLine(int id, math::Vector3d &_startPoint, math::Vector3d &_endPoint, math::Vector4d &_color)
/////////////////////////////////////////////////
void TapeMeasure::DrawLine(int _id, math::Vector3d &_startPoint,
math::Vector3d &_endPoint, math::Vector4d &_color)
{
ignition::msgs::Marker markerMsg;
if (this->dataPtr->placedMarkers.find(id) != this->dataPtr->placedMarkers.end())
{
// Delete the previously created marker
markerMsg.set_ns("default");
markerMsg.set_id(id);
markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER);
this->dataPtr->node.Request("/marker", markerMsg);
}
else
{
this->dataPtr->placedMarkers.insert(id);
}
this->DeleteMarker(_id);

markerMsg.set_ns("default");
markerMsg.set_id(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);
markerMsg.mutable_material()->mutable_ambient()->set_r(_color[0]);
@@ -203,6 +226,7 @@ void TapeMeasure::DrawLine(int id, math::Vector3d &_startPoint, math::Vector3d &
this->dataPtr->node.Request("/marker", markerMsg);
}

/////////////////////////////////////////////////
bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event)
{
if (_event->type() == ignition::gazebo::gui::events::HoverToScene::kType)
@@ -215,15 +239,19 @@ bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event)
if (this->dataPtr->measure && hoverToSceneEvent)
{
math::Vector3d point = hoverToSceneEvent->Point();
this->DrawPoint(this->dataPtr->currentId, point, this->dataPtr->hoverColor);
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->endPointId)
{
this->DrawLine(this->dataPtr->lineId, this->dataPtr->startPoint, point, this->dataPtr->hoverColor);
this->DrawLine(this->dataPtr->lineId, this->dataPtr->startPoint,
point, this->dataPtr->hoverColor);
this->dataPtr->distance = this->dataPtr->startPoint.Distance(point);
this->newDistance();
}
}
this->dataPtr->placed = true;
}
// Note: the following isn't an else if statement due to the hover scene
// event sometimes smothering this click event if the logic uses an else if
@@ -238,24 +266,27 @@ bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event)
if (this->dataPtr->measure && leftClickToSceneEvent)
{
math::Vector3d point = leftClickToSceneEvent->Point();
this->DrawPoint(this->dataPtr->currentId, point, this->dataPtr->drawColor);
// If we just placed the end point, end execution
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->startPointId)
{
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->lineId, this->dataPtr->startPoint, this->dataPtr->endPoint, this->dataPtr->drawColor);
ignwarn << "Distance is " << this->dataPtr->startPoint.Distance(this->dataPtr->endPoint) << "\n";
this->dataPtr->distance = this->dataPtr->startPoint.Distance(this->dataPtr->endPoint);
this->DrawLine(this->dataPtr->lineId, this->dataPtr->startPoint,
this->dataPtr->endPoint, this->dataPtr->drawColor);
this->dataPtr->distance =
this->dataPtr->startPoint.Distance(this->dataPtr->endPoint);
this->newDistance();
}
this->dataPtr->currentId = this->dataPtr->endPointId;
}
this->dataPtr->placed = true;
}

return QObject::eventFilter(_obj, _event);
43 changes: 39 additions & 4 deletions src/gui/plugins/tape_measure/TapeMeasure.hh
Original file line number Diff line number Diff line change
@@ -47,17 +47,52 @@ namespace gazebo
// Documentation inherited
public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override;

/// \brief Deletes the marker with the provided id within the
/// "tape_measure" namespace.
/// \param[in] _id The id of the marker
public: void DeleteMarker(int _id);

/// \brief Resets all of the relevant data for this plugin. Called when
/// the user clicks the reset button and when the user starts a new
/// measurement.
public: void Reset();

/// \brief Draws a point marker. Called to display the start and end
/// point of the tape measure.
/// \param[in] _id The id of the marker
/// \param[in] _point The x, y, z coordinates of where to place the marker
/// \param[in] _color The rgba color to set the marker
public: void DrawPoint(int _id,
ignition::math::Vector3d &_point,
ignition::math::Vector4d &_color);

/// \brief Draws a line marker. Called to display the line between the
/// start and end point of the tape measure.
/// \param[in] _id The id of the marker
/// \param[in] _startPoint The x, y, z coordinates of the line start point
/// \param[in] _endPoint The x, y, z coordinates of the line end point
/// \param[in] _color The rgba color to set the marker
public: void DrawLine(int _id,
ignition::math::Vector3d &_startPoint,
ignition::math::Vector3d &_endPoint,
ignition::math::Vector4d &_color);

/// \brief Callback in Qt thread when the new measurement button is
/// clicked.
public slots: void OnMeasure();

/// \brief Callback in Qt thread when the reset button is clicked.
public slots: void OnReset();
public slots: double Distance();
public: void Reset();

public: void DrawPoint(int id, ignition::math::Vector3d &_point, ignition::math::Vector4d &_color);
public: void DrawLine(int id, ignition::math::Vector3d &_startPoint, ignition::math::Vector3d &_endPoint, ignition::math::Vector4d &_color);
/// \brief Callback in Qt thread to get the distance to display in the
/// gui window.
/// \return The distance between the start and end point of the measurement
public slots: double Distance();

// Documentation inherited
protected: bool eventFilter(QObject *_obj, QEvent *_event) override;

/// \brief Signal fired when a new tape measure distance is set.
signals: void newDistance();

/// \internal