Skip to content

Commit

Permalink
Allow user to modify joint type (#1198)
Browse files Browse the repository at this point in the history
* Support editing air pressure sensor in the GUI

Signed-off-by: Nate Koenig <[email protected]>

* Add noise to qrc

Signed-off-by: Nate Koenig <[email protected]>

* Add noise to qrc

Signed-off-by: Nate Koenig <[email protected]>

* Fix lint

Signed-off-by: Michael Carroll <[email protected]>

* Update sensor icon

Signed-off-by: Nate Koenig <[email protected]>

* Move AirPressure functions out of ComponentInspector (#1179)

Signed-off-by: Louise Poubel <[email protected]>

* Fix get decimals, and address comments

Signed-off-by: Nate Koenig <[email protected]>

* cleanup and simplification

Signed-off-by: Nate Koenig <[email protected]>

* Require sdf 12.1.0

Signed-off-by: Nate Koenig <[email protected]>

* missign width

Signed-off-by: Nate Koenig <[email protected]>

* Added simulation state aware spin box

Signed-off-by: Nate Koenig <[email protected]>

* Remove console output

Signed-off-by: Nate Koenig <[email protected]>

* Allow user to modify joint type

Signed-off-by: Michael Carroll <[email protected]>

* Updated to use a separate class, and consolidate the look

Signed-off-by: Nate Koenig <[email protected]>

* Added recreate to joint add

Signed-off-by: Nate Koenig <[email protected]>

Co-authored-by: Nate Koenig <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
3 people authored Dec 1, 2021
1 parent d392d0f commit 9d0471f
Show file tree
Hide file tree
Showing 8 changed files with 296 additions and 0 deletions.
2 changes: 2 additions & 0 deletions src/gui/plugins/component_inspector/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@ gz_add_gui_plugin(ComponentInspector
Altimeter.cc
ComponentInspector.cc
Imu.cc
JointType.cc
Lidar.cc
Magnetometer.cc
ModelEditor.cc
Expand All @@ -13,6 +14,7 @@ gz_add_gui_plugin(ComponentInspector
Altimeter.hh
ComponentInspector.hh
Imu.hh
JointType.hh
Lidar.hh
Magnetometer.hh
ModelEditor.hh
Expand Down
7 changes: 7 additions & 0 deletions src/gui/plugins/component_inspector/ComponentInspector.cc
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@
#include "Altimeter.hh"
#include "ComponentInspector.hh"
#include "Imu.hh"
#include "JointType.hh"
#include "Lidar.hh"
#include "Magnetometer.hh"
#include "ModelEditor.hh"
Expand Down Expand Up @@ -130,6 +131,9 @@ namespace ignition::gazebo
/// \brief Imu inspector elements
public: std::unique_ptr<ignition::gazebo::Imu> imu;

/// \brief Joint inspector elements
public: std::unique_ptr<ignition::gazebo::JointType> joint;

/// \brief Lidar inspector elements
public: std::unique_ptr<ignition::gazebo::Lidar> lidar;

Expand Down Expand Up @@ -458,6 +462,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
// Create the imu
this->dataPtr->imu = std::make_unique<Imu>(this);

// Create the joint
this->dataPtr->joint = std::make_unique<JointType>(this);

// Create the lidar
this->dataPtr->lidar = std::make_unique<Lidar>(this);

Expand Down
1 change: 1 addition & 0 deletions src/gui/plugins/component_inspector/ComponentInspector.hh
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <string>

#include <sdf/Physics.hh>
#include <sdf/Joint.hh>

#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
Expand Down
1 change: 1 addition & 0 deletions src/gui/plugins/component_inspector/ComponentInspector.qrc
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
<file>ComponentInspector.qml</file>
<file>ExpandingTypeHeader.qml</file>
<file>Imu.qml</file>
<file>JointType.qml</file>
<file>Lidar.qml</file>
<file>LidarScan.qml</file>
<file>Light.qml</file>
Expand Down
124 changes: 124 additions & 0 deletions src/gui/plugins/component_inspector/JointType.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,124 @@
/*
* Copyright (C) 2021 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 <sdf/Joint.hh>

#include <ignition/common/Console.hh>
#include "ignition/gazebo/components/JointType.hh"
#include "ignition/gazebo/components/ParentEntity.hh"
#include "ignition/gazebo/components/Recreate.hh"
#include <ignition/gazebo/EntityComponentManager.hh>

#include "JointType.hh"
#include "ComponentInspector.hh"
#include "Types.hh"

using namespace ignition;
using namespace gazebo;

/////////////////////////////////////////////////
JointType::JointType(ComponentInspector *_inspector)
{
_inspector->Context()->setContextProperty("JointTypeImpl", this);
this->inspector = _inspector;

ComponentCreator creator =
[=](EntityComponentManager &_ecm, Entity _entity, QStandardItem *_item)
{
auto comp = _ecm.Component<components::JointType>(_entity);
if (nullptr == _item || nullptr == comp)
return;

const sdf::JointType joint = comp->Data();

_item->setData(QString("JointType"),
ComponentsModel::RoleNames().key("dataType"));

QString jointType;
if (joint == sdf::JointType::BALL)
jointType = "Ball";
else if (joint == sdf::JointType::CONTINUOUS)
jointType = "Continuous";
else if (joint == sdf::JointType::FIXED)
jointType = "Fixed";
else if (joint == sdf::JointType::GEARBOX)
jointType = "Gearbox";
else if (joint == sdf::JointType::PRISMATIC)
jointType = "Prismatic";
else if (joint == sdf::JointType::REVOLUTE)
jointType = "Revolute";
else if (joint == sdf::JointType::REVOLUTE2)
jointType = "Revolute2";
else if (joint == sdf::JointType::SCREW)
jointType = "Screw";
else if (joint == sdf::JointType::UNIVERSAL)
jointType = "Universal";

_item->setData(jointType, ComponentsModel::RoleNames().key("data"));
};

this->inspector->RegisterComponentCreator(
components::JointType::typeId, creator);
}

/////////////////////////////////////////////////
Q_INVOKABLE void JointType::OnJointType(QString _jointType)
{
ignition::gazebo::UpdateCallback cb =
[=](EntityComponentManager &_ecm)
{
components::JointType *comp =
_ecm.Component<components::JointType>(this->inspector->Entity());

components::ParentEntity *parentComp =
_ecm.Component<components::ParentEntity>(this->inspector->Entity());

if (comp && parentComp)
{
if (_jointType == "Ball")
comp->Data() = sdf::JointType::BALL;
else if (_jointType == "Continuous")
comp->Data() = sdf::JointType::CONTINUOUS;
else if (_jointType == "Fixed")
comp->Data() = sdf::JointType::FIXED;
else if (_jointType == "Gearbox")
comp->Data() = sdf::JointType::GEARBOX;
else if (_jointType == "Prismatic")
comp->Data() = sdf::JointType::PRISMATIC;
else if (_jointType == "Revolute")
comp->Data() = sdf::JointType::REVOLUTE;
else if (_jointType == "Revolute2")
comp->Data() = sdf::JointType::REVOLUTE2;
else if (_jointType == "Screw")
comp->Data() = sdf::JointType::SCREW;
else if (_jointType == "Universal")
comp->Data() = sdf::JointType::UNIVERSAL;

// Make sure to mark the parent as needing recreation. This will
// tell the server to rebuild the model with the new link.
_ecm.CreateComponent(parentComp->Data(), components::Recreate());
}
else if (!comp)
{
ignerr << "Unable to get the joint type component.\n";
}
else
{
ignerr << "Unable to get the joint's parent entity component.\n";
}
};
this->inspector->AddUpdateCallback(cb);
}
47 changes: 47 additions & 0 deletions src/gui/plugins/component_inspector/JointType.hh
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
/*
* Copyright (C) 2021 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_COMPONENTINSPECTOR_JOINT_HH_
#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_JOINT_HH_

#include <ignition/gazebo/gui/GuiSystem.hh>

namespace ignition
{
namespace gazebo
{
class ComponentInspector;

/// \brief A class that handles joint changes.
class JointType : public QObject
{
Q_OBJECT

/// \brief Constructor
/// \param[in] _inspector The component inspector.
public: explicit JointType(ComponentInspector *_inspector);

/// \brief Callback in Qt thread when joint type changes.
/// \param[in] _surface Surface model
public: Q_INVOKABLE void OnJointType(QString _jointType);

/// \brief Pointer to the component inspector. This is used to add
/// update callbacks that modify the ECM.
private: ComponentInspector *inspector{nullptr};
};
}
}
#endif
108 changes: 108 additions & 0 deletions src/gui/plugins/component_inspector/JointType.qml
Original file line number Diff line number Diff line change
@@ -0,0 +1,108 @@
/*
* Copyright (C) 2021 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 1.4
import QtQuick.Controls 2.2
import QtQuick.Controls.Material 2.1
import QtQuick.Layouts 1.3
import QtQuick.Controls.Styles 1.4
import "qrc:/ComponentInspector"
import "qrc:/qml"

// Item displaying joint type information.
Rectangle {
id: jointTypeComponent
height: jointType.height
width: componentInspector.width
color: index % 2 == 0 ? lightGrey : darkGrey

// Left indentation
property int indentation: 10

// Horizontal margins
property int margin: 5

ListModel {
id: jointTypes
ListElement { type: "Ball" }
ListElement { type: "Continuous" }
ListElement { type: "Fixed" }
ListElement { type: "Gearbox" }
ListElement { type: "Prismatic" }
ListElement { type: "Revolute" }
ListElement { type: "Revolute2" }
ListElement { type: "Screw" }
ListElement { type: "Universal" }
}

function indexFromModel(_model) {
if (_model && _model.data !== undefined)
{
for (var i = 0; i < jointTypes.count; i++)
{
if (jointTypes.get(i).type === _model.data)
{
return i
}
}
}
return -1
}

FontMetrics {
id: fontMetrics
font.family: "Roboto"
}

RowLayout {
anchors.fill: parent

Item {
height: parent.height
width: margin + indentation
}

TypeHeader {
id: typeHeader
}

QtObject{
// Workaround to keep from using the ListModel as model
id: indexObj
property int index: indexFromModel(model)
}

ComboBox {
id: jointType
padding: 0
textRole: "type"
model: jointTypes
currentIndex: indexObj.index
Layout.alignment: Qt.AlignRight
background: Rectangle {
color: "transparent"
implicitWidth: 140
border.width: 1
border.color: "#dedede"
}
enabled: componentInspector.getSimPaused()
onActivated: {
JointTypeImpl.OnJointType(currentText)
}
}
}
}
6 changes: 6 additions & 0 deletions src/gui/plugins/component_inspector/ModelEditor.cc
Original file line number Diff line number Diff line change
Expand Up @@ -225,6 +225,12 @@ void ModelEditor::Update(const UpdateInfo &,
auto entity =
this->dataPtr->entityCreator->CreateEntities(&jointSdf, true);
this->dataPtr->entityCreator->SetParent(entity, eta.parentEntity);
// Make sure to mark the parent as needing recreation. This will
// tell the server to rebuild the model with the new link.
_ecm.CreateComponent(eta.parentEntity, components::Recreate());

// traverse the tree and add all new entities created by the entity
// creator to the set
entities.push_back(entity);
}
}
Expand Down

0 comments on commit 9d0471f

Please sign in to comment.