From 1817a9ad193e2076bdb823d32da0151d6ef366d2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Tue, 7 Jul 2020 12:23:16 +0200 Subject: [PATCH 1/8] Initial fix: visualize ContactSensorData MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- examples/worlds/visualize_contacts.sdf | 381 ++++++++++++++++++ src/gui/plugins/CMakeLists.txt | 1 + .../plugins/visualize_contacts/CMakeLists.txt | 8 + .../visualize_contacts/VisualizeContacts.cc | 295 ++++++++++++++ .../visualize_contacts/VisualizeContacts.hh | 76 ++++ .../visualize_contacts/VisualizeContacts.qml | 98 +++++ .../visualize_contacts/VisualizeContacts.qrc | 5 + src/systems/physics/Physics.cc | 44 ++ 8 files changed, 908 insertions(+) create mode 100644 examples/worlds/visualize_contacts.sdf create mode 100644 src/gui/plugins/visualize_contacts/CMakeLists.txt create mode 100644 src/gui/plugins/visualize_contacts/VisualizeContacts.cc create mode 100644 src/gui/plugins/visualize_contacts/VisualizeContacts.hh create mode 100644 src/gui/plugins/visualize_contacts/VisualizeContacts.qml create mode 100644 src/gui/plugins/visualize_contacts/VisualizeContacts.qrc diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf new file mode 100644 index 0000000000..2caf1e135a --- /dev/null +++ b/examples/worlds/visualize_contacts.sdf @@ -0,0 +1,381 @@ + + + + + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + /world/visualize_contacts/control + /world/visualize_contacts/stats + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + /world/visualize_contacts/stats + + + + + + + 0 + 0 + 263 + 50 + floating + false + #03a9f4 + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + -0.3 0 1.15 0 0 0 + + + + + 0.3 + 0.8 + + + + + + + 0.3 + 0.8 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + collision + + + + + + + 0 0 0.325 0 0 0 + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + 0.5 1.0 0.5 1 + + + + + + 2.01142 1 0.568726 + + + + + + collision + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + collision + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + collision + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + collision + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index c2ef4a81b0..6588d782d5 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -89,4 +89,5 @@ add_subdirectory(scene3d) add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) +add_subdirectory(visualize_contacts) add_subdirectory(view_angle) diff --git a/src/gui/plugins/visualize_contacts/CMakeLists.txt b/src/gui/plugins/visualize_contacts/CMakeLists.txt new file mode 100644 index 0000000000..3e377f6599 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(VisualizeContacts + SOURCES + VisualizeContacts.cc + QT_HEADERS + VisualizeContacts.hh + PUBLIC_LINK_LIBS + ${IGNITION-RENDERING_LIBRARIES} +) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc new file mode 100644 index 0000000000..39e04c1fe9 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -0,0 +1,295 @@ +/* + * 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 +#include + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" + +#include "VisualizeContacts.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + /// \brief Private data class for VisualizeContacts + class VisualizeContactsPrivate + { + /// \brief Transport node + public: transport::Node node; + + /// \brief Current state of the checkbox + public: bool checkboxState{false}; + + /// \brief Message for visualizing contact positions + public: ignition::msgs::Marker positionMarkerMsg; + + /// \brief Radius of the visualized contact sphere + public: double contactRadius{0.10}; + + /// \brief Message for visualizing contact forces + public: ignition::msgs::Marker forceMarkerMsg; + + /// \brief Length of the visualized force cylinder + public: double forceLenght{0.40}; + + /// \brief Update time of the markers in milliseconds + public: int64_t markerLifetime{350}; + + /// \brief Simulation time for the last markers update + public: std::chrono::steady_clock::duration lastMarkersUpdateTime{0}; + + /// \brief Mutex for variable mutated by the checkbox and spinboxes + /// callbacks. + /// The variables are: checkboxState, contactRadius and forceLenght. + public: std::mutex serviceMutex; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +VisualizeContacts::VisualizeContacts() + : GuiSystem(), dataPtr(new VisualizeContactsPrivate) +{ +} + +///////////////////////////////////////////////// +VisualizeContacts::~VisualizeContacts() = default; + +///////////////////////////////////////////////// +void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Visualize contacts"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); + + // Configure Marker messages for position and force of the contacts + + // Blue spheres for positions + // Green cylinders for forces + + // Create the marker message + this->dataPtr->positionMarkerMsg.set_ns("positions"); + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->positionMarkerMsg.set_type( + ignition::msgs::Marker::SPHERE); + this->dataPtr->positionMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()-> + set_sec(0); + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->markerLifetime * 1000000); + + this->dataPtr->forceMarkerMsg.set_ns("forces"); + this->dataPtr->forceMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->forceMarkerMsg.set_type( + ignition::msgs::Marker::CYLINDER); + this->dataPtr->forceMarkerMsg.set_visibility( + ignition::msgs::Marker::GUI); + this->dataPtr-> + forceMarkerMsg.mutable_lifetime()-> + set_sec(0); + this->dataPtr-> + forceMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->markerLifetime * 1000000); + + // Set material properties + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); + this->dataPtr-> + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); + this->dataPtr-> + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + + // Set scales + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); + + ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), + ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); +} + +///////////////////////////////////////////////// +void VisualizeContacts::OnVisualize(bool _checked) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->checkboxState = _checked; +} + +////////////////////////////////////////////////// +void VisualizeContacts::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("VisualizeContacts::Update"); + + { + std::lock_guard lock(this->dataPtr->serviceMutex); + if (!this->dataPtr->checkboxState) + return; + } + + // Only publish markers if enough time has passed + auto timeDiff = + std::chrono::duration_cast(_info.simTime - + this->dataPtr->lastMarkersUpdateTime); + + if (timeDiff.count() < this->dataPtr->markerLifetime) + return; + + // Store simulation time + this->dataPtr->lastMarkersUpdateTime = _info.simTime; + + // todo(anyone) Get the contacts of the links that don't have a + // contact sensor + + // Get the contacts and publish them + // Since we are setting a lifetime for the markers, we get all the + // contacts instead of getting news and removed ones + _ecm.Each( + [&](const Entity &, + const components::ContactSensorData *_contacts) -> bool + { + for (const auto contact : _contacts->Data().contact()) + { + // todo(anyone) add information about contact normal, depth + // and wrench + for (int i = 0; i < contact.position_size(); ++i) + { + // Set marker poses and request service + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_pose(), + ignition::math::Pose3d(contact.position(i).x(), + contact.position(i).y(), contact.position(i).z(), + 0, 0, 0)); + + // The position of the force marker is modified in order to place the + // end of the cylinder in the contact point, not its middle point + ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_pose(), + ignition::math::Pose3d(contact.position(i).x(), + contact.position(i).y(), + contact.position(i).z() + this->dataPtr->forceLenght/2, + 0, 0, 0)); + + this->dataPtr->node.Request( + "/marker", this->dataPtr->positionMarkerMsg); + this->dataPtr->node.Request( + "/marker", this->dataPtr->forceMarkerMsg); + } + } + return true; + }); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdateLength(double _length) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->forceLenght = _length; + + // Set scale + ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), + ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdateRadius(double _radius) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->contactRadius = _radius; + + // Set scale + ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::VisualizeContacts, + ignition::gui::Plugin) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh new file mode 100644 index 0000000000..b185216060 --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh @@ -0,0 +1,76 @@ +/* + * 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. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_VISUALIZECONTACTS_HH_ +#define IGNITION_GAZEBO_GUI_VISUALIZECONTACTS_HH_ + +#include + +#include + +#include "ignition/gui/qt.h" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE +{ + class VisualizeContactsPrivate; + + /// \brief Visualize the contacts returned by the Physics plugin. Use the + /// checkbox to turn visualization on or off and spin boxes to change + /// the size of the markers. + class VisualizeContacts : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: VisualizeContacts(); + + /// \brief Destructor + public: ~VisualizeContacts() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback when checkbox state is changed + /// \param[in] _checked indicates show or hide contacts + public slots: void OnVisualize(bool _checked); + + /// \brief Update the radius of the contact + /// \param[in] _radius new radius of the contact + public slots: void UpdateRadius(double _radius); + + /// \brief Update the length of the contact + /// \param[in] _length new length of the contact + public slots: void UpdateLength(double _length); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml new file mode 100644 index 0000000000..756b4700ce --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -0,0 +1,98 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 6 + columnSpacing: 10 + Layout.minimumWidth: 200 + Layout.minimumHeight: 200 + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Left spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + CheckBox { + Layout.alignment: Qt.AlignHCenter + id: visualizeContacts + Layout.columnSpan: 4 + text: qsTr("Show/Hide Contacts") + checked: false + onClicked: { + VisualizeContacts.OnVisualize(checked) + } + } + + // Right spacer + Item { + Layout.columnSpan: 1 + Layout.rowSpan: 15 + Layout.fillWidth: true + } + + Text { + Layout.columnSpan: 4 + text: "Markers size" + color: "dimgrey" + font.bold: true + } + + Text { + Layout.columnSpan: 2 + id: radiusText + color: "dimgrey" + text: "Radius" + } + + IgnSpinBox { + Layout.columnSpan: 2 + id: radius + maximumValue: 2.00 + minimumValue: 0.01 + value: 0.10 + decimals: 2 + stepSize: 0.05 + onEditingFinished: VisualizeContacts.UpdateRadius(radius.value) + } + + Text { + Layout.columnSpan: 2 + id: lengthText + color: "dimgrey" + text: "Length" + } + + IgnSpinBox { + Layout.columnSpan: 2 + id: length + maximumValue: 2.00 + minimumValue: 0.01 + value: 0.40 + decimals: 2 + stepSize: 0.05 + onEditingFinished: VisualizeContacts.UpdateLength(length.value) + } +} \ No newline at end of file diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc b/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc new file mode 100644 index 0000000000..fe1bf74e0a --- /dev/null +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qrc @@ -0,0 +1,5 @@ + + + VisualizeContacts.qml + + diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 3b7cb1caf6..636a409311 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -240,6 +240,42 @@ class ignition::gazebo::systems::PhysicsPrivate return _a == _b; }}; + /// \brief msgs::Contacts equality comparison function. + public: std::function + contactsEql { [](const msgs::Contacts &_a, + const msgs::Contacts &_b) + { + if (_a.contact_size() != _b.contact_size()) + { + return false; + } + + for (int i = 0 ; i < _a.contact_size() ; ++i) + { + if (_a.contact(i).position_size() != + _b.contact(i).position_size()) + { + return false; + } + + for (int j = 0 ; j < _a.contact(i).position_size() ; + ++j) + { + auto pos1 = _a.contact(i).position(j); + auto pos2 = _b.contact(i).position(j); + + if (!math::equal(pos1.x(), pos2.x(), 1e-6) || + !math::equal(pos1.y(), pos2.y(), 1e-6) || + !math::equal(pos1.z(), pos2.z(), 1e-6)) + { + return false; + } + } + } + return true; + }}; + /// \brief Environment variable which holds paths to look for engine plugins public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH"; @@ -1942,6 +1978,14 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) position->set_z(contact->point.z()); } } + + auto state = _contacts->SetData(contactsComp, + this->contactsEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + _ecm.SetChanged( + _collEntity1, components::ContactSensorData::typeId, state); + *_contacts = components::ContactSensorData(contactsComp); return true; From a7677cb4eff763346f4103ec22ea1e335185270e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 27 Jul 2020 12:14:38 +0200 Subject: [PATCH 2/8] Add services for enabling and disabling collision components MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- examples/worlds/visualize_contacts.sdf | 15 - .../visualize_contacts/VisualizeContacts.cc | 211 +++++++++---- .../visualize_contacts/VisualizeContacts.hh | 11 +- .../visualize_contacts/VisualizeContacts.qml | 30 +- src/systems/physics/Physics.cc | 48 +-- src/systems/user_commands/UserCommands.cc | 284 ++++++++++++++++++ 6 files changed, 497 insertions(+), 102 deletions(-) diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf index 2caf1e135a..5db0ca25d2 100644 --- a/examples/worlds/visualize_contacts.sdf +++ b/examples/worlds/visualize_contacts.sdf @@ -222,11 +222,6 @@ Contacts will be visualized as blue spheres and green cylinders. - - - collision - - @@ -300,11 +295,6 @@ Contacts will be visualized as blue spheres and green cylinders. - - - collision - - @@ -339,11 +329,6 @@ Contacts will be visualized as blue spheres and green cylinders. - - - collision - - diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc index 39e04c1fe9..f703237b21 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -63,6 +63,11 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Private data class for VisualizeContacts class VisualizeContactsPrivate { + /// \brief Creates ContactSensorData for Collision components without a + /// Contact Sensor by requesting the /enable_contact service + /// \param[in] Reference to the GUI Entity Component Manager + public: void CreateCollisionData(EntityComponentManager &_ecm); + /// \brief Transport node public: transport::Node node; @@ -78,19 +83,27 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Message for visualizing contact forces public: ignition::msgs::Marker forceMarkerMsg; - /// \brief Length of the visualized force cylinder - public: double forceLenght{0.40}; + /// \brief Scale of the visualized force cylinder. By scale we mean how + /// many meters per newton the marker will be long + public: double forceScale{0.40}; /// \brief Update time of the markers in milliseconds - public: int64_t markerLifetime{350}; + public: int64_t markerLifetime{200}; /// \brief Simulation time for the last markers update public: std::chrono::steady_clock::duration lastMarkersUpdateTime{0}; /// \brief Mutex for variable mutated by the checkbox and spinboxes /// callbacks. - /// The variables are: checkboxState, contactRadius and forceLenght. + /// The variables are: checkboxState, contactRadius, forceScale and + /// markerLifetime public: std::mutex serviceMutex; + + /// \brief Initialization flag + public: bool initialized{false}; + + /// \brief Name of the world + public: std::string worldName; }; } } @@ -115,7 +128,7 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) this->title = "Visualize contacts"; ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->installEventFilter(this); + ignition::gui::MainWindow *>()->installEventFilter(this); // Configure Marker messages for position and force of the contacts @@ -125,75 +138,66 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) // Create the marker message this->dataPtr->positionMarkerMsg.set_ns("positions"); this->dataPtr->positionMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); + ignition::msgs::Marker::ADD_MODIFY); this->dataPtr->positionMarkerMsg.set_type( - ignition::msgs::Marker::SPHERE); + ignition::msgs::Marker::SPHERE); this->dataPtr->positionMarkerMsg.set_visibility( - ignition::msgs::Marker::GUI); + ignition::msgs::Marker::GUI); this->dataPtr-> - positionMarkerMsg.mutable_lifetime()-> - set_sec(0); + positionMarkerMsg.mutable_lifetime()-> + set_sec(0); this->dataPtr-> - positionMarkerMsg.mutable_lifetime()-> - set_nsec(this->dataPtr->markerLifetime * 1000000); + positionMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->markerLifetime * 1000000); this->dataPtr->forceMarkerMsg.set_ns("forces"); - this->dataPtr->forceMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->forceMarkerMsg.set_type( - ignition::msgs::Marker::CYLINDER); - this->dataPtr->forceMarkerMsg.set_visibility( - ignition::msgs::Marker::GUI); - this->dataPtr-> - forceMarkerMsg.mutable_lifetime()-> - set_sec(0); - this->dataPtr-> - forceMarkerMsg.mutable_lifetime()-> - set_nsec(this->dataPtr->markerLifetime * 1000000); + this->dataPtr->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); + this->dataPtr->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_sec(0); + this->dataPtr->forceMarkerMsg.mutable_lifetime()-> + set_nsec(this->dataPtr->markerLifetime * 1000000); // Set material properties this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); + positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); + positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); + forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); + forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); + forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); + forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - // Set scales + // Set contact position scale ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->dataPtr->contactRadius, - this->dataPtr->contactRadius, - this->dataPtr->contactRadius)); - - ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); } ///////////////////////////////////////////////// @@ -215,6 +219,27 @@ void VisualizeContacts::Update(const UpdateInfo &_info, return; } + if (!this->dataPtr->initialized) + { + // Get the name of the world + if (this->dataPtr->worldName.empty()) + { + _ecm.Each( + [&](const Entity &, + const components::World *, + const components::Name *_name) -> bool + { + // We assume there's only one world + this->dataPtr->worldName = _name->Data(); + return false; + }); + } + + // Enable collisions + this->dataPtr->CreateCollisionData(_ecm); + this->dataPtr->initialized = true; + } + // Only publish markers if enough time has passed auto timeDiff = std::chrono::duration_cast(_info.simTime - @@ -232,28 +257,47 @@ void VisualizeContacts::Update(const UpdateInfo &_info, // Get the contacts and publish them // Since we are setting a lifetime for the markers, we get all the // contacts instead of getting news and removed ones + + // Variable for setting the markers id through the iteration + int markerID = 1; _ecm.Each( [&](const Entity &, const components::ContactSensorData *_contacts) -> bool { - for (const auto contact : _contacts->Data().contact()) + for (const auto &contact : _contacts->Data().contact()) { // todo(anyone) add information about contact normal, depth // and wrench for (int i = 0; i < contact.position_size(); ++i) { - // Set marker poses and request service + // Skip dummy data set by physics + bool dataIsDummy = std::fabs( + contact.position(i).x() - + static_cast(ignition::math::NAN_I)) < 0.001; + if (dataIsDummy) + return true; + + // Set marker id, poses and request service + this->dataPtr->positionMarkerMsg.set_id(markerID); ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_pose(), ignition::math::Pose3d(contact.position(i).x(), contact.position(i).y(), contact.position(i).z(), 0, 0, 0)); + // Placeholder for the force value (see todo comment above) + double force = 1; + + double forceLength = this->dataPtr->forceScale * force; + ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), + ignition::math::Vector3d(0.02, 0.02, forceLength)); + // The position of the force marker is modified in order to place the // end of the cylinder in the contact point, not its middle point + this->dataPtr->forceMarkerMsg.set_id(markerID++); ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_pose(), ignition::math::Pose3d(contact.position(i).x(), contact.position(i).y(), - contact.position(i).z() + this->dataPtr->forceLenght/2, + contact.position(i).z() + forceLength/2, 0, 0, 0)); this->dataPtr->node.Request( @@ -267,14 +311,49 @@ void VisualizeContacts::Update(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void VisualizeContacts::UpdateLength(double _length) +void VisualizeContactsPrivate::CreateCollisionData( + EntityComponentManager &_ecm) { - std::lock_guard lock(this->dataPtr->serviceMutex); - this->dataPtr->forceLenght = _length; + // Collisions can't be enabled with _ecm given that this is a GUI plugin and + // it doesn't run in the same process as the physics. + // We use the world//enable_collision service instead. + _ecm.Each( + [&](const Entity &_entity, + const components::Collision *) -> bool + { + // Check if ContactSensorData has already been created + bool collisionHasContactSensor = + _ecm.EntityHasComponentType(_entity, + components::ContactSensorData::typeId); - // Set scale - ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.02, 0.02, this->dataPtr->forceLenght)); + if (collisionHasContactSensor) + { + igndbg << "ContactSensorData detected in collision [" << _entity << "]" + << std::endl; + return true; + } + + // Request service for enabling collision + msgs::Entity req; + req.set_id(_entity); + req.set_type(msgs::Entity::COLLISION); + + msgs::Boolean res; + bool result; + unsigned int timeout = 50; + std::string service = "/world/" + this->worldName + "/enable_collision"; + + this->node.Request(service, req, timeout, res, result); + + return true; + }); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdateScale(double _scale) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->forceScale = _scale; } ////////////////////////////////////////////////// @@ -285,9 +364,23 @@ void VisualizeContacts::UpdateRadius(double _radius) // Set scale ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), - ignition::math::Vector3d(this->dataPtr->contactRadius, - this->dataPtr->contactRadius, - this->dataPtr->contactRadius)); + ignition::math::Vector3d(this->dataPtr->contactRadius, + this->dataPtr->contactRadius, + this->dataPtr->contactRadius)); +} + +////////////////////////////////////////////////// +void VisualizeContacts::UpdatePeriod(double _period) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->markerLifetime = _period; + + // Set markers lifetime + this->dataPtr-> + positionMarkerMsg.mutable_lifetime()->set_nsec(_period * 1000000); + + this->dataPtr-> + forceMarkerMsg.mutable_lifetime()->set_nsec(_period * 1000000); } // Register this plugin diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh index b185216060..c8bc04580a 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh @@ -61,9 +61,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in] _radius new radius of the contact public slots: void UpdateRadius(double _radius); - /// \brief Update the length of the contact - /// \param[in] _length new length of the contact - public slots: void UpdateLength(double _length); + /// \brief Update the scale of the contact. By scale we mean how many + /// meters per newton the marker will be long + /// \param[in] _scale new scale of the contact + public slots: void UpdateScale(double _scale); + + /// \brief Update the update period of the markers + /// \param[in] _period new update period + public slots: void UpdatePeriod(double _period); /// \internal /// \brief Pointer to private data diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml index 756b4700ce..7f8853719b 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -39,7 +39,7 @@ GridLayout { Layout.alignment: Qt.AlignHCenter id: visualizeContacts Layout.columnSpan: 4 - text: qsTr("Show/Hide Contacts") + text: qsTr("Show Contacts") checked: false onClicked: { VisualizeContacts.OnVisualize(checked) @@ -64,7 +64,7 @@ GridLayout { Layout.columnSpan: 2 id: radiusText color: "dimgrey" - text: "Radius" + text: "Radius (m)" } IgnSpinBox { @@ -80,19 +80,37 @@ GridLayout { Text { Layout.columnSpan: 2 - id: lengthText + id: scaleText color: "dimgrey" - text: "Length" + text: "Force scale (m/N)" } IgnSpinBox { Layout.columnSpan: 2 - id: length + id: scale maximumValue: 2.00 minimumValue: 0.01 value: 0.40 decimals: 2 stepSize: 0.05 - onEditingFinished: VisualizeContacts.UpdateLength(length.value) + onEditingFinished: VisualizeContacts.UpdateScale(scale.value) + } + + Text { + Layout.columnSpan: 2 + id: updatePeriodText + color: "dimgrey" + text: "Update period (ms)" + } + + IgnSpinBox { + Layout.columnSpan: 2 + id: updatePeriod + maximumValue: 2000 + minimumValue: 10 + value: 200 + decimals: 0 + stepSize: 50 + onEditingFinished: VisualizeContacts.UpdatePeriod(updatePeriod.value) } } \ No newline at end of file diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 636a409311..a2b76e9eff 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -251,15 +251,15 @@ class ignition::gazebo::systems::PhysicsPrivate return false; } - for (int i = 0 ; i < _a.contact_size() ; ++i) + for (int i = 0; i < _a.contact_size(); ++i) { if (_a.contact(i).position_size() != _b.contact(i).position_size()) { - return false; + return false; } - for (int j = 0 ; j < _a.contact(i).position_size() ; + for (int j = 0; j < _a.contact(i).position_size(); ++j) { auto pos1 = _a.contact(i).position(j); @@ -1954,28 +1954,40 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) [&](const Entity &_collEntity1, components::Collision *, components::ContactSensorData *_contacts) -> bool { + msgs::Contacts contactsComp; if (entityContactMap.find(_collEntity1) == entityContactMap.end()) { // Clear the last contact data *_contacts = components::ContactSensorData(); - return true; - } - const auto &contactMap = entityContactMap[_collEntity1]; - - msgs::Contacts contactsComp; - - for (const auto &[collEntity2, contactData] : contactMap) - { + // If there's an empty ContactSensorData, it won't be updated in the + // GUI ECM and there could be remaining contacts which shouldn't + // exist anymore. We insert dummy data in the empty ContactSensorData + // in order to avoid this problem. msgs::Contact *contactMsg = contactsComp.add_contact(); contactMsg->mutable_collision1()->set_id(_collEntity1); - contactMsg->mutable_collision2()->set_id(collEntity2); - for (const auto &contact : contactData) + contactMsg->mutable_collision2()->set_id(_collEntity1); + auto *position = contactMsg->add_position(); + position->set_x(ignition::math::NAN_I); + position->set_y(ignition::math::NAN_I); + position->set_z(ignition::math::NAN_I); + } + else + { + const auto &contactMap = entityContactMap[_collEntity1]; + + for (const auto &[collEntity2, contactData] : contactMap) { - auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + msgs::Contact *contactMsg = contactsComp.add_contact(); + contactMsg->mutable_collision1()->set_id(_collEntity1); + contactMsg->mutable_collision2()->set_id(collEntity2); + for (const auto &contact : contactData) + { + auto *position = contactMsg->add_position(); + position->set_x(contact->point.x()); + position->set_y(contact->point.y()); + position->set_z(contact->point.z()); + } } } @@ -1986,8 +1998,6 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) _ecm.SetChanged( _collEntity1, components::ContactSensorData::typeId, state); - *_contacts = components::ContactSensorData(contactsComp); - return true; }); } diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 3e4bab0b82..a33b960e36 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -38,6 +38,9 @@ #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "UserCommands.hh" @@ -141,6 +144,46 @@ class PoseCommand : public UserCommandBase math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); }}; }; + +/// \brief Command to enable a collision component. +class EnableCollisionCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message identifying the collision to be enabled. + /// \param[in] _iface Pointer to user commands interface. + public: EnableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief Check if there's a contact sensor connected to a collision + /// component + /// \param[in] _collision Collision entity to be checked + /// \return True if a Contact sensor is connected to the collision entity, + /// false otherwise + public: bool HasContactSensor(const Entity _collision); +}; + +/// \brief Command to disable a collision component. +class DisableCollisionCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message identifying the collision to be disabled. + /// \param[in] _iface Pointer to user commands interface. + public: DisableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief Check if there's a contact sensor connected to a collision + /// component + /// \param[in] _collision Collision entity to be checked + /// \return True if a Contact sensor is connected to the collision entity, + /// false otherwise + public: bool HasContactSensor(const Entity _collision); +}; } } } @@ -180,6 +223,22 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res); + /// \brief Callback for enable collision service + /// \param[in] _req Request containing collision entity. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the collision will be successfully enabled. + /// \return True if successful. + public: bool EnableCollisionService( + const msgs::Entity &_req, msgs::Boolean &_res); + + /// \brief Callback for disable collision service + /// \param[in] _req Request containing collision entity. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the collision will be successfully disabled. + /// \return True if successful. + public: bool DisableCollisionService( + const msgs::Entity &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -243,6 +302,24 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::PoseService, this->dataPtr.get()); ignmsg << "Pose service on [" << poseService << "]" << std::endl; + + // Enable collision service + std::string enableCollisionService{ + "/world/" + worldName + "/enable_collision"}; + this->dataPtr->node.Advertise(enableCollisionService, + &UserCommandsPrivate::EnableCollisionService, this->dataPtr.get()); + + ignmsg << "Enable collision service on [" << enableCollisionService << "]" + << std::endl; + + // Disable collision service + std::string disableCollisionService{ + "/world/" + worldName + "/disable_collision"}; + this->dataPtr->node.Advertise(disableCollisionService, + &UserCommandsPrivate::DisableCollisionService, this->dataPtr.get()); + + ignmsg << "Disable collision service on [" << disableCollisionService << "]" + << std::endl; } ////////////////////////////////////////////////// @@ -354,6 +431,44 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::EnableCollisionService(const msgs::Entity &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + +////////////////////////////////////////////////// +bool UserCommandsPrivate::DisableCollisionService(const msgs::Entity &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -683,6 +798,175 @@ bool PoseCommand::Execute() return true; } +////////////////////////////////////////////////// +EnableCollisionCommand::EnableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool EnableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Create ContactSensorData component + auto contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (contactDataComp) + { + ignwarn << "Can't create component that already exists" << std::endl; + return false; + } + + this->iface->ecm-> + CreateComponent(entityMsg->id(), components::ContactSensorData()); + igndbg << "Enabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + +////////////////////////////////////////////////// +bool EnableCollisionCommand::HasContactSensor(const Entity _collision) +{ + auto *ecm = this->iface->ecm; + + auto *linkEntity = ecm->Component(_collision); + auto allLinkSensors = + ecm->EntitiesByComponents(components::Sensor(), + components::ParentEntity(*linkEntity)); + + for (auto const &sensor : allLinkSensors) + { + // Check if it is a contact sensor + auto isContactSensor = + ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); + if (!isContactSensor) + continue; + + // Check if sensor is connected to _collision + auto componentName = ecm->Component(_collision); + std::string collisionName = componentName->Data(); + auto sensorSDF = ecm->Component(sensor)->Data(); + auto sensorCollisionName = + sensorSDF->GetElement("contact")->Get("collision"); + + if (collisionName == sensorCollisionName) + { + return true; + } + } + + return false; +} + +////////////////////////////////////////////////// +DisableCollisionCommand::DisableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool DisableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Remove ContactSensorData component + auto *contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (!contactDataComp) + { + ignwarn << "No ContactSensorData detected inside entity " << entityMsg->id() + << std::endl; + return false; + } + + this->iface->ecm-> + RemoveComponent(entityMsg->id(), components::ContactSensorData::typeId); + + igndbg << "Disabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + +////////////////////////////////////////////////// +bool DisableCollisionCommand::HasContactSensor(const Entity _collision) +{ + auto *ecm = this->iface->ecm; + + auto *linkEntity = ecm->Component(_collision); + auto allLinkSensors = + ecm->EntitiesByComponents(components::Sensor(), + components::ParentEntity(*linkEntity)); + + for (auto const &sensor : allLinkSensors) + { + // Check if it is a contact sensor + auto isContactSensor = + ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); + if (!isContactSensor) + continue; + + // Check if sensor is connected to _collision + auto componentName = ecm->Component(_collision); + std::string collisionName = componentName->Data(); + auto sensorSDF = ecm->Component(sensor)->Data(); + auto sensorCollisionName = + sensorSDF->GetElement("contact")->Get("collision"); + + if (collisionName == sensorCollisionName) + { + return true; + } + } + + return false; +} IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, From 1898b18fa0a5ef4eac355b16390b4fa6acdec8c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Mon, 27 Jul 2020 14:08:48 +0200 Subject: [PATCH 3/8] PR Feedback 1 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../visualize_contacts/VisualizeContacts.cc | 37 ++++++++++++++++--- .../visualize_contacts/VisualizeContacts.qml | 13 +++---- 2 files changed, 36 insertions(+), 14 deletions(-) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc index f703237b21..e9cbfde542 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -74,6 +74,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Current state of the checkbox public: bool checkboxState{false}; + /// \brief Previous state of the checkbox + public: bool checkboxPrevState{false}; + /// \brief Message for visualizing contact positions public: ignition::msgs::Marker positionMarkerMsg; @@ -213,12 +216,6 @@ void VisualizeContacts::Update(const UpdateInfo &_info, { IGN_PROFILE("VisualizeContacts::Update"); - { - std::lock_guard lock(this->dataPtr->serviceMutex); - if (!this->dataPtr->checkboxState) - return; - } - if (!this->dataPtr->initialized) { // Get the name of the world @@ -240,6 +237,34 @@ void VisualizeContacts::Update(const UpdateInfo &_info, this->dataPtr->initialized = true; } + { + std::lock_guard lock(this->dataPtr->serviceMutex); + if (this->dataPtr->checkboxPrevState && !this->dataPtr->checkboxState) + { + // Remove the markers + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::DELETE_ALL); + this->dataPtr->forceMarkerMsg.set_action( + ignition::msgs::Marker::DELETE_ALL); + + igndbg << "Removing markers..." << std::endl; + this->dataPtr->node.Request( + "/marker", this->dataPtr->positionMarkerMsg); + this->dataPtr->node.Request( + "/marker", this->dataPtr->forceMarkerMsg); + + // Change action in case checkbox is checked again + this->dataPtr->positionMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->forceMarkerMsg.set_action( + ignition::msgs::Marker::ADD_MODIFY); + } + + this->dataPtr->checkboxPrevState = this->dataPtr->checkboxState; + if (!this->dataPtr->checkboxState) + return; + } + // Only publish markers if enough time has passed auto timeDiff = std::chrono::duration_cast(_info.simTime - diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml index 7f8853719b..85ff043850 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -23,8 +23,9 @@ import "qrc:/qml" GridLayout { columns: 6 columnSpacing: 10 - Layout.minimumWidth: 200 + Layout.minimumWidth: 250 Layout.minimumHeight: 200 + anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -53,13 +54,6 @@ GridLayout { Layout.fillWidth: true } - Text { - Layout.columnSpan: 4 - text: "Markers size" - color: "dimgrey" - font.bold: true - } - Text { Layout.columnSpan: 2 id: radiusText @@ -69,6 +63,7 @@ GridLayout { IgnSpinBox { Layout.columnSpan: 2 + Layout.fillWidth: true id: radius maximumValue: 2.00 minimumValue: 0.01 @@ -87,6 +82,7 @@ GridLayout { IgnSpinBox { Layout.columnSpan: 2 + Layout.fillWidth: true id: scale maximumValue: 2.00 minimumValue: 0.01 @@ -105,6 +101,7 @@ GridLayout { IgnSpinBox { Layout.columnSpan: 2 + Layout.fillWidth: true id: updatePeriod maximumValue: 2000 minimumValue: 10 From 87bd92b1dab21cd50546903cec019677d38f0dae Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 20 Aug 2020 18:53:24 +0200 Subject: [PATCH 4/8] Minor refactoring for improving readability MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- .../visualize_contacts/VisualizeContacts.cc | 46 ++++++------------- 1 file changed, 13 insertions(+), 33 deletions(-) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc index e9cbfde542..9aeba8b4cb 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -162,39 +162,19 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) set_nsec(this->dataPtr->markerLifetime * 1000000); // Set material properties - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_g(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_b(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_g(0); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_b(1); - this->dataPtr-> - positionMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); - - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_r(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_g(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_b(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_ambient()->set_a(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_r(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_g(1); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_b(0); - this->dataPtr-> - forceMarkerMsg.mutable_material()->mutable_diffuse()->set_a(1); + ignition::msgs::Set( + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + ignition::msgs::Set( + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 1, 0, 1)); + ignition::msgs::Set( + this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 1, 0, 1)); // Set contact position scale ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), From d4ff151e33a0f932ea87eae98762a556adfa0a23 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Thu, 20 Aug 2020 18:55:08 +0200 Subject: [PATCH 5/8] Fix qml MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- src/gui/plugins/visualize_contacts/VisualizeContacts.qml | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml index 85ff043850..32ce0a41ed 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -24,7 +24,7 @@ GridLayout { columns: 6 columnSpacing: 10 Layout.minimumWidth: 250 - Layout.minimumHeight: 200 + Layout.minimumHeight: 400 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -110,4 +110,10 @@ GridLayout { stepSize: 50 onEditingFinished: VisualizeContacts.UpdatePeriod(updatePeriod.value) } + + // Bottom spacer + Item { + Layout.columnSpan: 4 + Layout.fillHeight: true + } } \ No newline at end of file From 1030417c25c8d282244050b2c068053c32e642ba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marti=C3=B1o=20Crespo?= Date: Wed, 23 Sep 2020 17:22:55 +0200 Subject: [PATCH 6/8] PR Feedback 2 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Martiño Crespo --- src/gui/plugins/CMakeLists.txt | 2 +- .../visualize_contacts/VisualizeContacts.cc | 47 +++---- .../visualize_contacts/VisualizeContacts.qml | 5 +- src/systems/user_commands/UserCommands.cc | 125 ++++++------------ 4 files changed, 61 insertions(+), 118 deletions(-) diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 85e4280096..9af87fe574 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -90,5 +90,5 @@ add_subdirectory(scene3d) add_subdirectory(shapes) add_subdirectory(transform_control) add_subdirectory(video_recorder) -add_subdirectory(visualize_contacts) add_subdirectory(view_angle) +add_subdirectory(visualize_contacts) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc index 9aeba8b4cb..fdc03da7a2 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -15,8 +15,8 @@ * */ -#include #include +#include #include #include @@ -24,33 +24,27 @@ #include #include -#include -#include -#include -#include #include -#include -#include #include -#include #include +#include #include -#include #include +#include #include +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" #include "ignition/gazebo/rendering/RenderUtil.hh" -#include "ignition/gazebo/components/Collision.hh" -#include "ignition/gazebo/components/ContactSensor.hh" -#include "ignition/gazebo/components/ContactSensorData.hh" #include "VisualizeContacts.hh" @@ -130,9 +124,6 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Visualize contacts"; - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->installEventFilter(this); - // Configure Marker messages for position and force of the contacts // Blue spheres for positions @@ -256,12 +247,9 @@ void VisualizeContacts::Update(const UpdateInfo &_info, // Store simulation time this->dataPtr->lastMarkersUpdateTime = _info.simTime; - // todo(anyone) Get the contacts of the links that don't have a - // contact sensor - // Get the contacts and publish them // Since we are setting a lifetime for the markers, we get all the - // contacts instead of getting news and removed ones + // contacts instead of getting new and removed ones // Variable for setting the markers id through the iteration int markerID = 1; @@ -271,25 +259,18 @@ void VisualizeContacts::Update(const UpdateInfo &_info, { for (const auto &contact : _contacts->Data().contact()) { - // todo(anyone) add information about contact normal, depth - // and wrench for (int i = 0; i < contact.position_size(); ++i) { - // Skip dummy data set by physics - bool dataIsDummy = std::fabs( - contact.position(i).x() - - static_cast(ignition::math::NAN_I)) < 0.001; - if (dataIsDummy) - return true; - // Set marker id, poses and request service - this->dataPtr->positionMarkerMsg.set_id(markerID); + this->dataPtr->positionMarkerMsg.set_id(markerID++); ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_pose(), ignition::math::Pose3d(contact.position(i).x(), contact.position(i).y(), contact.position(i).z(), 0, 0, 0)); - // Placeholder for the force value (see todo comment above) + // todo(anyone) add information about contact normal, depth + // and wrench in the following commented code + /* double force = 1; double forceLength = this->dataPtr->forceScale * force; @@ -305,10 +286,12 @@ void VisualizeContacts::Update(const UpdateInfo &_info, contact.position(i).z() + forceLength/2, 0, 0, 0)); - this->dataPtr->node.Request( - "/marker", this->dataPtr->positionMarkerMsg); this->dataPtr->node.Request( "/marker", this->dataPtr->forceMarkerMsg); + */ + + this->dataPtr->node.Request( + "/marker", this->dataPtr->positionMarkerMsg); } } return true; diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml index 32ce0a41ed..32b3fe233f 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -16,7 +16,6 @@ */ import QtQuick 2.9 import QtQuick.Controls 2.1 -import QtQuick.Dialogs 1.0 import QtQuick.Layouts 1.3 import "qrc:/qml" @@ -73,6 +72,9 @@ GridLayout { onEditingFinished: VisualizeContacts.UpdateRadius(radius.value) } + // Uncomment when plugin is available to get the force information from the + // contact message + /* Text { Layout.columnSpan: 2 id: scaleText @@ -91,6 +93,7 @@ GridLayout { stepSize: 0.05 onEditingFinished: VisualizeContacts.UpdateScale(scale.value) } + */ Text { Layout.columnSpan: 2 diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index a33b960e36..6a60a2d760 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -68,6 +68,13 @@ class UserCommandsInterface /// \brief World entity. public: Entity worldEntity{kNullEntity}; + + /// \brief Check if there's a contact sensor connected to a collision + /// component + /// \param[in] _collision Collision entity to be checked + /// \return True if a contact sensor is connected to the collision entity, + /// false otherwise + public: bool HasContactSensor(const Entity _collision); }; /// \brief All user commands should inherit from this class so they can be @@ -156,13 +163,6 @@ class EnableCollisionCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; - - /// \brief Check if there's a contact sensor connected to a collision - /// component - /// \param[in] _collision Collision entity to be checked - /// \return True if a Contact sensor is connected to the collision entity, - /// false otherwise - public: bool HasContactSensor(const Entity _collision); }; /// \brief Command to disable a collision component. @@ -176,13 +176,6 @@ class DisableCollisionCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; - - /// \brief Check if there's a contact sensor connected to a collision - /// component - /// \param[in] _collision Collision entity to be checked - /// \return True if a Contact sensor is connected to the collision entity, - /// false otherwise - public: bool HasContactSensor(const Entity _collision); }; } } @@ -261,6 +254,38 @@ UserCommands::UserCommands() : System(), ////////////////////////////////////////////////// UserCommands::~UserCommands() = default; +////////////////////////////////////////////////// +bool UserCommandsInterface::HasContactSensor(const Entity _collision) +{ + auto *linkEntity = ecm->Component(_collision); + auto allLinkSensors = + ecm->EntitiesByComponents(components::Sensor(), + components::ParentEntity(*linkEntity)); + + for (auto const &sensor : allLinkSensors) + { + // Check if it is a contact sensor + auto isContactSensor = + ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); + if (!isContactSensor) + continue; + + // Check if sensor is connected to _collision + auto componentName = ecm->Component(_collision); + std::string collisionName = componentName->Data(); + auto sensorSDF = ecm->Component(sensor)->Data(); + auto sensorCollisionName = + sensorSDF->GetElement("contact")->Get("collision"); + + if (collisionName == sensorCollisionName) + { + return true; + } + } + + return false; +} + ////////////////////////////////////////////////// void UserCommands::Configure(const Entity &_entity, const std::shared_ptr &, @@ -824,7 +849,7 @@ bool EnableCollisionCommand::Execute() } // Check if collision is connected to a contact sensor - if (this->HasContactSensor(entityMsg->id())) + if (this->iface->HasContactSensor(entityMsg->id())) { ignwarn << "Requested collision is connected to a contact sensor, " << "exiting service..." << std::endl; @@ -848,40 +873,6 @@ bool EnableCollisionCommand::Execute() return true; } -////////////////////////////////////////////////// -bool EnableCollisionCommand::HasContactSensor(const Entity _collision) -{ - auto *ecm = this->iface->ecm; - - auto *linkEntity = ecm->Component(_collision); - auto allLinkSensors = - ecm->EntitiesByComponents(components::Sensor(), - components::ParentEntity(*linkEntity)); - - for (auto const &sensor : allLinkSensors) - { - // Check if it is a contact sensor - auto isContactSensor = - ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); - if (!isContactSensor) - continue; - - // Check if sensor is connected to _collision - auto componentName = ecm->Component(_collision); - std::string collisionName = componentName->Data(); - auto sensorSDF = ecm->Component(sensor)->Data(); - auto sensorCollisionName = - sensorSDF->GetElement("contact")->Get("collision"); - - if (collisionName == sensorCollisionName) - { - return true; - } - } - - return false; -} - ////////////////////////////////////////////////// DisableCollisionCommand::DisableCollisionCommand(msgs::Entity *_msg, std::shared_ptr &_iface) @@ -908,7 +899,7 @@ bool DisableCollisionCommand::Execute() } // Check if collision is connected to a contact sensor - if (this->HasContactSensor(entityMsg->id())) + if (this->iface->HasContactSensor(entityMsg->id())) { ignwarn << "Requested collision is connected to a contact sensor, " << "exiting service..." << std::endl; @@ -934,40 +925,6 @@ bool DisableCollisionCommand::Execute() return true; } -////////////////////////////////////////////////// -bool DisableCollisionCommand::HasContactSensor(const Entity _collision) -{ - auto *ecm = this->iface->ecm; - - auto *linkEntity = ecm->Component(_collision); - auto allLinkSensors = - ecm->EntitiesByComponents(components::Sensor(), - components::ParentEntity(*linkEntity)); - - for (auto const &sensor : allLinkSensors) - { - // Check if it is a contact sensor - auto isContactSensor = - ecm->EntityHasComponentType(sensor, components::ContactSensor::typeId); - if (!isContactSensor) - continue; - - // Check if sensor is connected to _collision - auto componentName = ecm->Component(_collision); - std::string collisionName = componentName->Data(); - auto sensorSDF = ecm->Component(sensor)->Data(); - auto sensorCollisionName = - sensorSDF->GetElement("contact")->Get("collision"); - - if (collisionName == sensorCollisionName) - { - return true; - } - } - - return false; -} - IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate From 1e42e2e99aa5a6d4d0fbb16cb76ee4f1032dd6d2 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 7 Dec 2020 11:40:36 -0600 Subject: [PATCH 7/8] Don't skip of if component is not set This fixes a test failure where the expected number of contacts is zero, but we were sending dummy data to workaround an ECM synchronization issue. Signed-off-by: Addisu Z. Taddese --- src/EntityComponentManager.cc | 7 ------- src/systems/physics/Physics.cc | 20 +++++++------------- 2 files changed, 7 insertions(+), 20 deletions(-) diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index fb2d0c9bf6..476720db7e 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -1164,13 +1164,6 @@ void EntityComponentManager::SetState( { const auto &compMsg = compIter.second; - // Skip if component not set. Note that this will also skip components - // setting an empty value. - if (compMsg.component().empty()) - { - continue; - } - uint64_t type = compMsg.type(); // Components which haven't been registered in this process, such as 3rd diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c4c70b4849..a30e333c96 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2287,19 +2287,13 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) if (entityContactMap.find(_collEntity1) == entityContactMap.end()) { // Clear the last contact data - *_contacts = components::ContactSensorData(); - - // If there's an empty ContactSensorData, it won't be updated in the - // GUI ECM and there could be remaining contacts which shouldn't - // exist anymore. We insert dummy data in the empty ContactSensorData - // in order to avoid this problem. - msgs::Contact *contactMsg = contactsComp.add_contact(); - contactMsg->mutable_collision1()->set_id(_collEntity1); - contactMsg->mutable_collision2()->set_id(_collEntity1); - auto *position = contactMsg->add_position(); - position->set_x(ignition::math::NAN_I); - position->set_y(ignition::math::NAN_I); - position->set_z(ignition::math::NAN_I); + auto state = _contacts->SetData(contactsComp, + this->contactsEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + _ecm.SetChanged( + _collEntity1, components::ContactSensorData::typeId, state); + return true; } else { From c28fc2053deeea0c179171a90897e2e32d1f9936 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Mon, 3 May 2021 16:56:05 -0700 Subject: [PATCH 8/8] Remove forces, use valid topic Signed-off-by: Louise Poubel --- .../visualize_contacts/VisualizeContacts.cc | 66 +---------- .../visualize_contacts/VisualizeContacts.hh | 5 - .../visualize_contacts/VisualizeContacts.qml | 35 +----- src/systems/physics/Physics.cc | 26 ++--- src/systems/user_commands/UserCommands.cc | 107 +++++++++++++++++- 5 files changed, 124 insertions(+), 115 deletions(-) diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc index fdc03da7a2..4420d90d05 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.cc +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.cc @@ -77,13 +77,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Radius of the visualized contact sphere public: double contactRadius{0.10}; - /// \brief Message for visualizing contact forces - public: ignition::msgs::Marker forceMarkerMsg; - - /// \brief Scale of the visualized force cylinder. By scale we mean how - /// many meters per newton the marker will be long - public: double forceScale{0.40}; - /// \brief Update time of the markers in milliseconds public: int64_t markerLifetime{200}; @@ -92,8 +85,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \brief Mutex for variable mutated by the checkbox and spinboxes /// callbacks. - /// The variables are: checkboxState, contactRadius, forceScale and - /// markerLifetime + /// The variables are: checkboxState, contactRadius and markerLifetime public: std::mutex serviceMutex; /// \brief Initialization flag @@ -124,10 +116,9 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "Visualize contacts"; - // Configure Marker messages for position and force of the contacts + // Configure Marker messages for position of the contacts // Blue spheres for positions - // Green cylinders for forces // Create the marker message this->dataPtr->positionMarkerMsg.set_ns("positions"); @@ -144,14 +135,6 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) positionMarkerMsg.mutable_lifetime()-> set_nsec(this->dataPtr->markerLifetime * 1000000); - this->dataPtr->forceMarkerMsg.set_ns("forces"); - this->dataPtr->forceMarkerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->forceMarkerMsg.set_type(ignition::msgs::Marker::CYLINDER); - this->dataPtr->forceMarkerMsg.set_visibility(ignition::msgs::Marker::GUI); - this->dataPtr->forceMarkerMsg.mutable_lifetime()->set_sec(0); - this->dataPtr->forceMarkerMsg.mutable_lifetime()-> - set_nsec(this->dataPtr->markerLifetime * 1000000); - // Set material properties ignition::msgs::Set( this->dataPtr->positionMarkerMsg.mutable_material()->mutable_ambient(), @@ -160,13 +143,6 @@ void VisualizeContacts::LoadConfig(const tinyxml2::XMLElement *) this->dataPtr->positionMarkerMsg.mutable_material()->mutable_diffuse(), ignition::math::Color(0, 0, 1, 1)); - ignition::msgs::Set( - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_ambient(), - ignition::math::Color(0, 1, 0, 1)); - ignition::msgs::Set( - this->dataPtr->forceMarkerMsg.mutable_material()->mutable_diffuse(), - ignition::math::Color(0, 1, 0, 1)); - // Set contact position scale ignition::msgs::Set(this->dataPtr->positionMarkerMsg.mutable_scale(), ignition::math::Vector3d(this->dataPtr->contactRadius, @@ -215,20 +191,14 @@ void VisualizeContacts::Update(const UpdateInfo &_info, // Remove the markers this->dataPtr->positionMarkerMsg.set_action( ignition::msgs::Marker::DELETE_ALL); - this->dataPtr->forceMarkerMsg.set_action( - ignition::msgs::Marker::DELETE_ALL); igndbg << "Removing markers..." << std::endl; this->dataPtr->node.Request( "/marker", this->dataPtr->positionMarkerMsg); - this->dataPtr->node.Request( - "/marker", this->dataPtr->forceMarkerMsg); // Change action in case checkbox is checked again this->dataPtr->positionMarkerMsg.set_action( ignition::msgs::Marker::ADD_MODIFY); - this->dataPtr->forceMarkerMsg.set_action( - ignition::msgs::Marker::ADD_MODIFY); } this->dataPtr->checkboxPrevState = this->dataPtr->checkboxState; @@ -268,28 +238,6 @@ void VisualizeContacts::Update(const UpdateInfo &_info, contact.position(i).y(), contact.position(i).z(), 0, 0, 0)); - // todo(anyone) add information about contact normal, depth - // and wrench in the following commented code - /* - double force = 1; - - double forceLength = this->dataPtr->forceScale * force; - ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_scale(), - ignition::math::Vector3d(0.02, 0.02, forceLength)); - - // The position of the force marker is modified in order to place the - // end of the cylinder in the contact point, not its middle point - this->dataPtr->forceMarkerMsg.set_id(markerID++); - ignition::msgs::Set(this->dataPtr->forceMarkerMsg.mutable_pose(), - ignition::math::Pose3d(contact.position(i).x(), - contact.position(i).y(), - contact.position(i).z() + forceLength/2, - 0, 0, 0)); - - this->dataPtr->node.Request( - "/marker", this->dataPtr->forceMarkerMsg); - */ - this->dataPtr->node.Request( "/marker", this->dataPtr->positionMarkerMsg); } @@ -337,13 +285,6 @@ void VisualizeContactsPrivate::CreateCollisionData( }); } -////////////////////////////////////////////////// -void VisualizeContacts::UpdateScale(double _scale) -{ - std::lock_guard lock(this->dataPtr->serviceMutex); - this->dataPtr->forceScale = _scale; -} - ////////////////////////////////////////////////// void VisualizeContacts::UpdateRadius(double _radius) { @@ -366,9 +307,6 @@ void VisualizeContacts::UpdatePeriod(double _period) // Set markers lifetime this->dataPtr-> positionMarkerMsg.mutable_lifetime()->set_nsec(_period * 1000000); - - this->dataPtr-> - forceMarkerMsg.mutable_lifetime()->set_nsec(_period * 1000000); } // Register this plugin diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh index c8bc04580a..f1c6211084 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.hh +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.hh @@ -61,11 +61,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in] _radius new radius of the contact public slots: void UpdateRadius(double _radius); - /// \brief Update the scale of the contact. By scale we mean how many - /// meters per newton the marker will be long - /// \param[in] _scale new scale of the contact - public slots: void UpdateScale(double _scale); - /// \brief Update the update period of the markers /// \param[in] _period new update period public slots: void UpdatePeriod(double _period); diff --git a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml index 32b3fe233f..0562257e92 100644 --- a/src/gui/plugins/visualize_contacts/VisualizeContacts.qml +++ b/src/gui/plugins/visualize_contacts/VisualizeContacts.qml @@ -72,29 +72,6 @@ GridLayout { onEditingFinished: VisualizeContacts.UpdateRadius(radius.value) } - // Uncomment when plugin is available to get the force information from the - // contact message - /* - Text { - Layout.columnSpan: 2 - id: scaleText - color: "dimgrey" - text: "Force scale (m/N)" - } - - IgnSpinBox { - Layout.columnSpan: 2 - Layout.fillWidth: true - id: scale - maximumValue: 2.00 - minimumValue: 0.01 - value: 0.40 - decimals: 2 - stepSize: 0.05 - onEditingFinished: VisualizeContacts.UpdateScale(scale.value) - } - */ - Text { Layout.columnSpan: 2 id: updatePeriodText @@ -114,9 +91,9 @@ GridLayout { onEditingFinished: VisualizeContacts.UpdatePeriod(updatePeriod.value) } - // Bottom spacer - Item { - Layout.columnSpan: 4 - Layout.fillHeight: true - } -} \ No newline at end of file + // Bottom spacer + Item { + Layout.columnSpan: 4 + Layout.fillHeight: true + } +} diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index f3e241ba5b..90447526f9 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -2384,22 +2384,20 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) _collEntity1, components::ContactSensorData::typeId, state); return true; } - else - { - const auto &contactMap = entityContactMap[_collEntity1]; - for (const auto &[collEntity2, contactData] : contactMap) + const auto &contactMap = entityContactMap[_collEntity1]; + + for (const auto &[collEntity2, contactData] : contactMap) + { + msgs::Contact *contactMsg = contactsComp.add_contact(); + contactMsg->mutable_collision1()->set_id(_collEntity1); + contactMsg->mutable_collision2()->set_id(collEntity2); + for (const auto &contact : contactData) { - msgs::Contact *contactMsg = contactsComp.add_contact(); - contactMsg->mutable_collision1()->set_id(_collEntity1); - contactMsg->mutable_collision2()->set_id(collEntity2); - for (const auto &contact : contactData) - { - auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); - } + auto *position = contactMsg->add_position(); + position->set_x(contact->point.x()); + position->set_y(contact->point.y()); + position->set_z(contact->point.z()); } } diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 718ce391c1..549aca774f 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -459,7 +459,7 @@ void UserCommands::Configure(const Entity &_entity, // Enable collision service std::string enableCollisionService{ - "/world/" + worldName + "/enable_collision"}; + "/world/" + validWorldName + "/enable_collision"}; this->dataPtr->node.Advertise(enableCollisionService, &UserCommandsPrivate::EnableCollisionService, this->dataPtr.get()); @@ -468,7 +468,7 @@ void UserCommands::Configure(const Entity &_entity, // Disable collision service std::string disableCollisionService{ - "/world/" + worldName + "/disable_collision"}; + "/world/" + validWorldName + "/disable_collision"}; this->dataPtr->node.Advertise(disableCollisionService, &UserCommandsPrivate::DisableCollisionService, this->dataPtr.get()); @@ -948,7 +948,6 @@ bool RemoveCommand::Execute() return true; } - ////////////////////////////////////////////////// LightCommand::LightCommand(msgs::Light *_msg, std::shared_ptr &_iface) @@ -1127,6 +1126,108 @@ bool PhysicsCommand::Execute() return true; } +////////////////////////////////////////////////// +EnableCollisionCommand::EnableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool EnableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->iface->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Create ContactSensorData component + auto contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (contactDataComp) + { + ignwarn << "Can't create component that already exists" << std::endl; + return false; + } + + this->iface->ecm-> + CreateComponent(entityMsg->id(), components::ContactSensorData()); + igndbg << "Enabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + +////////////////////////////////////////////////// +DisableCollisionCommand::DisableCollisionCommand(msgs::Entity *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool DisableCollisionCommand::Execute() +{ + auto entityMsg = dynamic_cast(this->msg); + if (nullptr == entityMsg) + { + ignerr << "Internal error, null create message" << std::endl; + return false; + } + + // Check Entity type + if (entityMsg->type() != msgs::Entity::COLLISION) + { + ignwarn << "Expected msgs::Entity::Type::COLLISION, exiting service..." + << std::endl; + return false; + } + + // Check if collision is connected to a contact sensor + if (this->iface->HasContactSensor(entityMsg->id())) + { + ignwarn << "Requested collision is connected to a contact sensor, " + << "exiting service..." << std::endl; + return false; + } + + // Remove ContactSensorData component + auto *contactDataComp = + this->iface->ecm->Component< + components::ContactSensorData>(entityMsg->id()); + if (!contactDataComp) + { + ignwarn << "No ContactSensorData detected inside entity " << entityMsg->id() + << std::endl; + return false; + } + + this->iface->ecm-> + RemoveComponent(entityMsg->id(), components::ContactSensorData::typeId); + + igndbg << "Disabled collision [" << entityMsg->id() << "]" << std::endl; + + return true; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate