diff --git a/examples/worlds/lights.sdf b/examples/worlds/lights.sdf
index f1910f0ac7..61d2d92fd1 100644
--- a/examples/worlds/lights.sdf
+++ b/examples/worlds/lights.sdf
@@ -52,6 +52,7 @@
0.01
false
+ false
diff --git a/src/Conversions.cc b/src/Conversions.cc
index bdc67fd9cb..549c1e527f 100644
--- a/src/Conversions.cc
+++ b/src/Conversions.cc
@@ -570,6 +570,25 @@ msgs::Light ignition::gazebo::convert(const sdf::Light &_in)
out.set_spot_inner_angle(_in.SpotInnerAngle().Radian());
out.set_spot_outer_angle(_in.SpotOuterAngle().Radian());
out.set_spot_falloff(_in.SpotFalloff());
+
+ {
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ auto header = out.mutable_header()->add_data();
+ header->set_key("isLightOn");
+ std::string *value = header->add_value();
+ *value = std::to_string(_in.LightOn());
+ }
+
+ {
+ // todo(ahcorde) Use the field visualize_visual in light.proto from
+ // Garden on.
+ auto header = out.mutable_header()->add_data();
+ header->set_key("visualizeVisual");
+ std::string *value = header->add_value();
+ *value = std::to_string(_in.Visualize());
+ }
+
if (_in.Type() == sdf::LightType::POINT)
out.set_type(msgs::Light_LightType_POINT);
else if (_in.Type() == sdf::LightType::SPOT)
@@ -599,6 +618,43 @@ sdf::Light ignition::gazebo::convert(const msgs::Light &_in)
out.SetSpotInnerAngle(math::Angle(_in.spot_inner_angle()));
out.SetSpotOuterAngle(math::Angle(_in.spot_outer_angle()));
out.SetSpotFalloff(_in.spot_falloff());
+
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ bool visualizeVisual = true;
+ for (int i = 0; i < _in.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _in.header().data(i).value_size(); ++j)
+ {
+ if (_in.header().data(i).key() ==
+ "visualizeVisual")
+ {
+ visualizeVisual = ignition::math::parseInt(
+ _in.header().data(i).value(0));
+ }
+ }
+ }
+ out.SetVisualize(visualizeVisual);
+
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ bool isLightOn = true;
+ for (int i = 0; i < _in.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _in.header().data(i).value_size(); ++j)
+ {
+ if (_in.header().data(i).key() ==
+ "isLightOn")
+ {
+ isLightOn = ignition::math::parseInt(
+ _in.header().data(i).value(0));
+ }
+ }
+ }
+ out.SetLightOn(isLightOn);
+
if (_in.type() == msgs::Light_LightType_POINT)
out.SetType(sdf::LightType::POINT);
else if (_in.type() == msgs::Light_LightType_SPOT)
diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt
index 367278b24d..d4bc795abc 100644
--- a/src/gui/plugins/component_inspector/CMakeLists.txt
+++ b/src/gui/plugins/component_inspector/CMakeLists.txt
@@ -1,4 +1,8 @@
gz_add_gui_plugin(ComponentInspector
- SOURCES ComponentInspector.cc
- QT_HEADERS ComponentInspector.hh
+ SOURCES
+ ComponentInspector.cc
+ Pose3d.cc
+ QT_HEADERS
+ ComponentInspector.hh
+ Pose3d.hh
)
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
index 5dbfff76b4..5bf8124493 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.cc
+++ b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -24,7 +24,6 @@
#include
#include
#include
-#include
#include "ignition/gazebo/components/Actor.hh"
#include "ignition/gazebo/components/AngularAcceleration.hh"
@@ -56,8 +55,6 @@
#include "ignition/gazebo/components/PerformerAffinity.hh"
#include "ignition/gazebo/components/Physics.hh"
#include "ignition/gazebo/components/PhysicsEnginePlugin.hh"
-#include "ignition/gazebo/components/Pose.hh"
-#include "ignition/gazebo/components/PoseCmd.hh"
#include "ignition/gazebo/components/RenderEngineGuiPlugin.hh"
#include "ignition/gazebo/components/RenderEngineServerPlugin.hh"
#include "ignition/gazebo/components/SelfCollide.hh"
@@ -75,6 +72,7 @@
#include "ignition/gazebo/gui/GuiEvents.hh"
#include "ComponentInspector.hh"
+#include "Pose3d.hh"
namespace ignition::gazebo
{
@@ -109,31 +107,19 @@ namespace ignition::gazebo
/// \brief Transport node for making command requests
public: transport::Node node;
+
+ /// \brief A map of component types to the function used to update it.
+ public: std::map
+ updateViewCbs;
+
+ /// \brief Handles all components displayed as a 3D pose.
+ public: std::unique_ptr pose3d;
};
}
using namespace ignition;
using namespace gazebo;
-//////////////////////////////////////////////////
-template<>
-void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data)
-{
- if (nullptr == _item)
- return;
-
- _item->setData(QString("Pose3d"),
- ComponentsModel::RoleNames().key("dataType"));
- _item->setData(QList({
- QVariant(_data.Pos().X()),
- QVariant(_data.Pos().Y()),
- QVariant(_data.Pos().Z()),
- QVariant(_data.Rot().Roll()),
- QVariant(_data.Rot().Pitch()),
- QVariant(_data.Rot().Yaw())
- }), ComponentsModel::RoleNames().key("data"));
-}
-
//////////////////////////////////////////////////
template<>
void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
@@ -155,6 +141,36 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
lightType = 2;
}
+ bool visualizeVisual = true;
+ for (int i = 0; i < _data.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _data.header().data(i).value_size(); ++j)
+ {
+ if (_data.header().data(i).key() ==
+ "visualizeVisual")
+ {
+ visualizeVisual = ignition::math::parseInt(
+ _data.header().data(i).value(0));
+ }
+ }
+ }
+
+ bool isLightOn = true;
+ for (int i = 0; i < _data.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _data.header().data(i).value_size(); ++j)
+ {
+ if (_data.header().data(i).key() ==
+ "isLightOn")
+ {
+ isLightOn = ignition::math::parseInt(
+ _data.header().data(i).value(0));
+ }
+ }
+ }
+
_item->setData(QString("Light"),
ComponentsModel::RoleNames().key("dataType"));
_item->setData(QList({
@@ -178,7 +194,9 @@ void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data)
QVariant(_data.spot_outer_angle()),
QVariant(_data.spot_falloff()),
QVariant(_data.intensity()),
- QVariant(lightType)
+ QVariant(lightType),
+ QVariant(isLightOn),
+ QVariant(visualizeVisual)
}), ComponentsModel::RoleNames().key("data"));
}
@@ -454,6 +472,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
// Connect model
this->Context()->setContextProperty(
"ComponentsModel", &this->dataPtr->componentsModel);
+
+ // Type-specific handlers
+ this->dataPtr->pose3d = std::make_unique(this);
}
//////////////////////////////////////////////////
@@ -737,12 +758,6 @@ void ComponentInspector::Update(const UpdateInfo &,
if (comp)
setData(item, comp->Data());
}
- else if (typeId == components::Pose::typeId)
- {
- auto comp = _ecm.Component(this->dataPtr->entity);
- if (comp)
- setData(item, comp->Data());
- }
else if (typeId == components::RenderEngineGuiPlugin::typeId)
{
auto comp = _ecm.Component(
@@ -864,18 +879,10 @@ void ComponentInspector::Update(const UpdateInfo &,
setUnit(item, "m/s");
}
}
- else if (typeId == components::WorldPose::typeId)
+ else if (this->dataPtr->updateViewCbs.find(typeId) !=
+ this->dataPtr->updateViewCbs.end())
{
- auto comp = _ecm.Component(this->dataPtr->entity);
- if (comp)
- setData(item, comp->Data());
- }
- else if (typeId == components::WorldPoseCmd::typeId)
- {
- auto comp = _ecm.Component(
- this->dataPtr->entity);
- if (comp)
- setData(item, comp->Data());
+ this->dataPtr->updateViewCbs[typeId](_ecm, item);
}
else if (typeId == components::Material::typeId)
{
@@ -909,6 +916,13 @@ void ComponentInspector::Update(const UpdateInfo &,
}
}
+/////////////////////////////////////////////////
+void ComponentInspector::AddUpdateViewCb(ComponentTypeId _id,
+ inspector::UpdateViewCb _cb)
+{
+ this->dataPtr->updateViewCbs[_id] = _cb;
+}
+
/////////////////////////////////////////////////
bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event)
{
@@ -998,26 +1012,6 @@ void ComponentInspector::SetPaused(bool _paused)
this->PausedChanged();
}
-/////////////////////////////////////////////////
-void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll,
- double _pitch, double _yaw)
-{
- std::function cb =
- [](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
- {
- if (!_result)
- ignerr << "Error setting pose" << std::endl;
- };
-
- ignition::msgs::Pose req;
- req.set_id(this->dataPtr->entity);
- msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
- msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
- auto poseCmdService = "/world/" + this->dataPtr->worldName
- + "/set_pose";
- this->dataPtr->node.Request(poseCmdService, req, cb);
-}
-
/////////////////////////////////////////////////
void ComponentInspector::OnLight(
double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular,
@@ -1025,7 +1019,8 @@ void ComponentInspector::OnLight(
double _attRange, double _attLinear, double _attConstant,
double _attQuadratic, bool _castShadows, double _directionX,
double _directionY, double _directionZ, double _innerAngle,
- double _outerAngle, double _falloff, double _intensity, int _type)
+ double _outerAngle, double _falloff, double _intensity, int _type,
+ bool _isLightOn, bool _visualizeVisual)
{
std::function cb =
[](const ignition::msgs::Boolean &/*_rep*/, const bool _result)
@@ -1035,6 +1030,23 @@ void ComponentInspector::OnLight(
};
ignition::msgs::Light req;
+ {
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ auto header = req.mutable_header()->add_data();
+ header->set_key("isLightOn");
+ std::string *value = header->add_value();
+ *value = std::to_string(_isLightOn);
+ }
+ {
+ // todo(ahcorde) Use the field visualize_visual in light.proto from
+ // Garden on.
+ auto header = req.mutable_header()->add_data();
+ header->set_key("visualizeVisual");
+ std::string *value = header->add_value();
+ *value = std::to_string(_visualizeVisual);
+ }
+
req.set_name(this->dataPtr->entityName);
req.set_id(this->dataPtr->entity);
ignition::msgs::Set(req.mutable_diffuse(),
@@ -1237,6 +1249,18 @@ bool ComponentInspector::NestedModel() const
return this->dataPtr->nestedModel;
}
+/////////////////////////////////////////////////
+const std::string &ComponentInspector::WorldName() const
+{
+ return this->dataPtr->worldName;
+}
+
+/////////////////////////////////////////////////
+transport::Node &ComponentInspector::TransportNode()
+{
+ return this->dataPtr->node;
+}
+
// Register this plugin
IGNITION_ADD_PLUGIN(ignition::gazebo::ComponentInspector,
ignition::gui::Plugin)
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh
index 2b8bbd50c7..a4800795e2 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.hh
+++ b/src/gui/plugins/component_inspector/ComponentInspector.hh
@@ -25,13 +25,15 @@
#include
#include
-#include
#include
+#include
#include
#include
#include
+#include "Types.hh"
+
#include
Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId)
@@ -69,12 +71,6 @@ namespace gazebo
template<>
void setData(QStandardItem *_item, const std::string &_data);
- /// \brief Specialized to set pose data.
- /// \param[in] _item Item whose data will be set.
- /// \param[in] _data Data to set.
- template<>
- void setData(QStandardItem *_item, const math::Pose3d &_data);
-
/// \brief Specialized to set light data.
/// \param[in] _item Item whose data will be set.
/// \param[in] _data Data to set.
@@ -228,15 +224,12 @@ namespace gazebo
// Documentation inherited
public: void Update(const UpdateInfo &, EntityComponentManager &) override;
- /// \brief Callback in Qt thread when pose changes.
- /// \param[in] _x X
- /// \param[in] _y Y
- /// \param[in] _z Z
- /// \param[in] _roll Roll
- /// \param[in] _pitch Pitch
- /// \param[in] _yaw Yaw
- public: Q_INVOKABLE void OnPose(double _x, double _y, double _z,
- double _roll, double _pitch, double _yaw);
+ /// \brief Add a callback that's called whenever there are updates from the
+ /// ECM to the view, for a given component type.
+ /// \param[in] _id The component type id
+ /// \param[in] _cb Function that's called when there are updates.
+ public: void AddUpdateViewCb(ComponentTypeId _id,
+ inspector::UpdateViewCb _cb);
/// \brief Callback in Qt thread when specular changes.
/// \param[in] _rSpecular specular red
@@ -260,6 +253,8 @@ namespace gazebo
/// \param[in] _falloff Falloff of the spotlight
/// \param[in] _intensity Intensity of the light
/// \param[in] _type light type
+ /// \param[in] _isLightOn is light on
+ /// \param[in] _visualizeVisual is visual enabled
public: Q_INVOKABLE void OnLight(
double _rSpecular, double _gSpecular, double _bSpecular,
double _aSpecular, double _rDiffuse, double _gDiffuse,
@@ -267,7 +262,8 @@ namespace gazebo
double _attLinear, double _attConstant, double _attQuadratic,
bool _castShadows, double _directionX, double _directionY,
double _directionZ, double _innerAngle, double _outerAngle,
- double _falloff, double _intensity, int _type);
+ double _falloff, double _intensity, int _type, bool _isLightOn,
+ bool _visualizeVisual);
/// \brief Callback in Qt thread when physics' properties change.
/// \param[in] _stepSize step size
@@ -368,6 +364,14 @@ namespace gazebo
/// \brief Notify that paused has changed.
signals: void PausedChanged();
+ /// \brief Name of world entity
+ /// \return World name
+ public: const std::string &WorldName() const;
+
+ /// \brief Node for communication
+ /// \return Transport node
+ public: transport::Node &TransportNode();
+
/// \internal
/// \brief Pointer to private data.
private: std::unique_ptr dataPtr;
diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml
index 53a77abfd3..90a1e53dcb 100644
--- a/src/gui/plugins/component_inspector/ComponentInspector.qml
+++ b/src/gui/plugins/component_inspector/ComponentInspector.qml
@@ -101,12 +101,14 @@ Rectangle {
_rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse,
_attRange, _attLinear, _attConstant, _attQuadratic,
_castShadows, _directionX, _directionY, _directionZ,
- _innerAngle, _outerAngle, _falloff, _intensity, _type) {
+ _innerAngle, _outerAngle, _falloff, _intensity, _type,
+ _isLightOn, _visualizeVisual) {
ComponentInspector.OnLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular,
_rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse,
_attRange, _attLinear, _attConstant, _attQuadratic,
_castShadows, _directionX, _directionY, _directionZ,
- _innerAngle, _outerAngle, _falloff, _intensity, _type)
+ _innerAngle, _outerAngle, _falloff, _intensity, _type,
+ _isLightOn, _visualizeVisual)
}
/*
diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml
index 26bc72806b..081b88972d 100644
--- a/src/gui/plugins/component_inspector/Light.qml
+++ b/src/gui/plugins/component_inspector/Light.qml
@@ -99,6 +99,12 @@ Rectangle {
// Loaded item for intensity
property var intensityItem: {}
+ // Loaded item for isLightOn
+ property var isLightOnItem: {}
+
+ // Loaded item for visualizeVisuals
+ property var visualizeVisualItem: {}
+
// Send new light data to C++
function sendLight() {
// TODO(anyone) There's a loss of precision when these values get to C++
@@ -123,7 +129,9 @@ Rectangle {
outerAngleItem.value,
falloffItem.value,
intensityItem.value,
- model.data[20]
+ model.data[20],
+ isLightOnItem.checked,
+ visualizeVisualItem.checked
);
}
@@ -248,6 +256,68 @@ Rectangle {
id: grid
width: parent.width
+ RowLayout {
+ Rectangle {
+ color: "transparent"
+ height: 40
+ Layout.preferredWidth: visualizeVisualText.width + indentation*3
+
+ Text {
+ id : visualizeVisualText
+ text: ' View gizmo'
+ leftPadding: 5
+ color: Material.theme == Material.Light ? "#444444" : "#bbbbbb"
+ font.pointSize: 12
+ anchors.centerIn: parent
+ }
+ }
+ Item {
+ Layout.fillWidth: true
+ height: 40
+
+ Loader {
+ id: visualizeVisualLoader
+ anchors.fill: parent
+ property double numberValue: model.data[22]
+ sourceComponent: ignSwitch
+ onLoaded: {
+ visualizeVisualItem = visualizeVisualLoader.item
+ }
+ }
+ }
+ }
+
+ RowLayout {
+ Rectangle {
+ color: "transparent"
+ height: 40
+ Layout.preferredWidth: isOnText.width + indentation*3
+
+ Text {
+ id : isOnText
+ text: ' Turn on/off'
+ leftPadding: 5
+ color: Material.theme == Material.Light ? "#444444" : "#bbbbbb"
+ font.pointSize: 12
+ anchors.centerIn: parent
+ }
+ }
+ Item {
+ Layout.fillWidth: true
+ height: 40
+
+ Loader {
+ id: isOnLoader
+ anchors.fill: parent
+ property double numberValue: model.data[21]
+ sourceComponent: ignSwitch
+ onLoaded: {
+ isLightOnItem = isOnLoader.item
+ }
+ }
+ }
+ }
+
RowLayout {
Rectangle {
color: "transparent"
diff --git a/src/gui/plugins/component_inspector/Pose3d.cc b/src/gui/plugins/component_inspector/Pose3d.cc
new file mode 100644
index 0000000000..e3a9446202
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Pose3d.cc
@@ -0,0 +1,64 @@
+/*
+ * 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
+
+#include
+#include
+
+#include "Pose3d.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace inspector;
+
+/////////////////////////////////////////////////
+Pose3d::Pose3d(ComponentInspector *_inspector)
+{
+ _inspector->Context()->setContextProperty("Pose3dImpl", this);
+ this->inspector = _inspector;
+
+ this->inspector->AddUpdateViewCb(components::Pose::typeId,
+ std::bind(&Pose3d::UpdateView, this,
+ std::placeholders::_1, std::placeholders::_2));
+ this->inspector->AddUpdateViewCb(components::WorldPose::typeId,
+ std::bind(&Pose3d::UpdateView, this,
+ std::placeholders::_1, std::placeholders::_2));
+ this->inspector->AddUpdateViewCb(components::WorldPoseCmd::typeId,
+ std::bind(&Pose3d::UpdateView, this,
+ std::placeholders::_1, std::placeholders::_2));
+}
+
+/////////////////////////////////////////////////
+void Pose3d::OnPose(double _x, double _y, double _z, double _roll,
+ double _pitch, double _yaw)
+{
+ std::function cb =
+ [](const msgs::Boolean &, const bool _result)
+ {
+ if (!_result)
+ ignerr << "Error setting pose" << std::endl;
+ };
+
+ msgs::Pose req;
+ req.set_id(this->inspector->GetEntity());
+ msgs::Set(req.mutable_position(), math::Vector3d(_x, _y, _z));
+ msgs::Set(req.mutable_orientation(), math::Quaterniond(_roll, _pitch, _yaw));
+ std::string poseCmdService("/world/" + this->inspector->WorldName()
+ + "/set_pose");
+ this->inspector->TransportNode().Request(poseCmdService, req, cb);
+}
diff --git a/src/gui/plugins/component_inspector/Pose3d.hh b/src/gui/plugins/component_inspector/Pose3d.hh
new file mode 100644
index 0000000000..890cac2590
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Pose3d.hh
@@ -0,0 +1,98 @@
+/*
+ * Copyright (C) 2022 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_POSE3D_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_POSE3D_HH_
+
+#include
+
+#include "ignition/gazebo/components/Pose.hh"
+#include "ignition/gazebo/components/PoseCmd.hh"
+#include "ignition/gazebo/EntityComponentManager.hh"
+
+#include "ComponentInspector.hh"
+#include "Types.hh"
+
+#include
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+class ComponentInspector;
+namespace inspector
+{
+ /// \brief Handles components that are displayed as a 3D pose:
+ /// * `components::Pose`
+ /// * `components::WorldPose`
+ /// * `components::WorldPoseCmd`
+ class Pose3d : public QObject
+ {
+ Q_OBJECT
+
+ /// \brief Constructor
+ /// \param[in] _inspector The component inspector.
+ public: explicit Pose3d(ComponentInspector *_inspector);
+
+ /// \brief Callback when there are ECM updates.
+ /// \param[in] _ecm Immutable reference to the ECM.
+ /// \param[in] _item Item to update.
+ /// \tparam ComponentType Type of component being updated.
+ public:
+ template
+ void UpdateView(const EntityComponentManager &_ecm,
+ QStandardItem *_item)
+ {
+ if (nullptr == _item)
+ return;
+
+ auto comp = _ecm.Component(this->inspector->GetEntity());
+ if (nullptr == comp)
+ return;
+
+ auto pose = comp->Data();
+
+ _item->setData(QString("Pose3d"),
+ ComponentsModel::RoleNames().key("dataType"));
+ _item->setData(QList({
+ QVariant(pose.Pos().X()),
+ QVariant(pose.Pos().Y()),
+ QVariant(pose.Pos().Z()),
+ QVariant(pose.Rot().Roll()),
+ QVariant(pose.Rot().Pitch()),
+ QVariant(pose.Rot().Yaw())
+ }), ComponentsModel::RoleNames().key("data"));
+ }
+
+ /// \brief Callback in Qt thread when pose changes.
+ /// \param[in] _x X
+ /// \param[in] _y Y
+ /// \param[in] _z Z
+ /// \param[in] _roll Roll
+ /// \param[in] _pitch Pitch
+ /// \param[in] _yaw Yaw
+ public: Q_INVOKABLE void OnPose(double _x, double _y, double _z,
+ double _roll, double _pitch, double _yaw);
+
+ /// \brief Pointer to the component inspector. This is used to add
+ /// callbacks.
+ private: ComponentInspector *inspector{nullptr};
+ };
+}
+}
+}
+#endif
diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml
index e9806463e4..0690edd458 100644
--- a/src/gui/plugins/component_inspector/Pose3d.qml
+++ b/src/gui/plugins/component_inspector/Pose3d.qml
@@ -68,7 +68,7 @@ Rectangle {
// Send new pose to C++
function sendPose() {
// TODO(anyone) There's a loss of precision when these values get to C++
- componentInspector.onPose(
+ Pose3dImpl.OnPose(
xItem.value,
yItem.value,
zItem.value,
diff --git a/src/gui/plugins/component_inspector/Types.hh b/src/gui/plugins/component_inspector/Types.hh
new file mode 100644
index 0000000000..5e0b682027
--- /dev/null
+++ b/src/gui/plugins/component_inspector/Types.hh
@@ -0,0 +1,42 @@
+/*
+ * Copyright (C) 2022 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_TYPES_HH_
+#define IGNITION_GAZEBO_GUI_COMPONENTINSPECTOR_TYPES_HH_
+
+#include
+
+#include
+
+namespace ignition
+{
+namespace gazebo
+{
+namespace inspector
+{
+ /// \brief Function definition that a component can use
+ /// to update its UI elements based on changes from the ECM.
+ /// * _ecm Immutable reference to the ECM
+ /// * _item Item to be updated
+ /// \sa ComponentInspector::AddUpdateViewCb
+ using UpdateViewCb = std::function;
+}
+}
+}
+#endif
+
diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc
index 759b5588e3..ad61ace576 100644
--- a/src/rendering/RenderUtil.cc
+++ b/src/rendering/RenderUtil.cc
@@ -378,6 +378,7 @@ class ignition::gazebo::RenderUtilPrivate
lightEql { [](const sdf::Light &_a, const sdf::Light &_b)
{
return
+ _a.Visualize() == _b.Visualize() &&
_a.Type() == _b.Type() &&
_a.Name() == _b.Name() &&
_a.Diffuse() == _b.Diffuse() &&
@@ -2811,11 +2812,58 @@ void RenderUtilPrivate::UpdateLights(
auto l = std::dynamic_pointer_cast(node);
if (l)
{
- if (!ignition::math::equal(
- l->Intensity(),
- static_cast(light.second.intensity())))
+ // todo(ahcorde) Use the field visualize_visual in light.proto from
+ // Garden on.
+ bool visualizeVisual = true;
+ for (int i = 0; i < light.second.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < light.second.header().data(i).value_size(); ++j)
+ {
+ if (light.second.header().data(i).key() ==
+ "visualizeVisual")
+ {
+ visualizeVisual = ignition::math::parseInt(
+ light.second.header().data(i).value(0));
+ }
+ }
+ }
+
+ rendering::VisualPtr lightVisual =
+ this->sceneManager.VisualById(
+ this->matchLightWithVisuals[light.first]);
+ if (lightVisual)
+ lightVisual->SetVisible(visualizeVisual);
+
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ bool isLightOn = true;
+ for (int i = 0; i < light.second.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < light.second.header().data(i).value_size(); ++j)
+ {
+ if (light.second.header().data(i).key() ==
+ "isLightOn")
+ {
+ isLightOn = ignition::math::parseInt(
+ light.second.header().data(i).value(0));
+ }
+ }
+ }
+
+ if (isLightOn)
+ {
+ if (!ignition::math::equal(
+ l->Intensity(),
+ static_cast(light.second.intensity())))
+ {
+ l->SetIntensity(light.second.intensity());
+ }
+ }
+ else
{
- l->SetIntensity(light.second.intensity());
+ l->SetIntensity(0);
}
if (light.second.has_diffuse())
{
diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
index ac6720231d..d72c663e6d 100644
--- a/src/rendering/SceneManager.cc
+++ b/src/rendering/SceneManager.cc
@@ -1245,6 +1245,9 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id,
lightVisual->SetInnerAngle(_light.SpotInnerAngle().Radian());
lightVisual->SetOuterAngle(_light.SpotOuterAngle().Radian());
}
+
+ lightVisual->SetVisible(_light.Visualize());
+
rendering::VisualPtr lightVis = std::dynamic_pointer_cast(
lightVisual);
lightVis->SetUserData("gazebo-entity", static_cast(_id));
diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc
index e37928eeb9..bdbacc24e2 100644
--- a/src/systems/user_commands/UserCommands.cc
+++ b/src/systems/user_commands/UserCommands.cc
@@ -22,6 +22,7 @@
#include
#include
#include
+#include
#include
#include
@@ -129,18 +130,6 @@ Entity topLevelEntityFromMessage(const EntityComponentManager &_ecm,
return kNullEntity;
}
-/// \brief Pose3d equality comparison function.
-/// \param[in] _a A pose to compare
-/// \param[in] _b Another pose to compare
-bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b)
-{
- return _a.Pos().Equal(_b.Pos(), 1e-6) &&
- math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
- math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
- math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
- math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
-}
-
/// \brief This class is passed to every command and contains interfaces that
/// can be shared among all commands. For example, all create and remove
/// commands can use the `creator` object.
@@ -230,7 +219,50 @@ class LightCommand : public UserCommandBase
public: std::function
lightEql { [](const msgs::Light &_a, const msgs::Light &_b)
{
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ auto getVisualizeVisual = [](const msgs::Light &_light) -> bool
+ {
+ bool visualizeVisual = true;
+ for (int i = 0; i < _light.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _light.header().data(i).value_size(); ++j)
+ {
+ if (_light.header().data(i).key() ==
+ "visualizeVisual")
+ {
+ visualizeVisual = ignition::math::parseInt(
+ _light.header().data(i).value(0));
+ }
+ }
+ }
+ return visualizeVisual;
+ };
+
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ auto getIsLightOn = [](const msgs::Light &_light) -> bool
+ {
+ bool isLightOn = true;
+ for (int i = 0; i < _light.header().data_size(); ++i)
+ {
+ for (int j = 0;
+ j < _light.header().data(i).value_size(); ++j)
+ {
+ if (_light.header().data(i).key() ==
+ "isLightOn")
+ {
+ isLightOn = ignition::math::parseInt(
+ _light.header().data(i).value(0));
+ }
+ }
+ }
+ return isLightOn;
+ };
return
+ getVisualizeVisual(_a) == getVisualizeVisual(_b) &&
+ getIsLightOn(_a) == getIsLightOn(_b) &&
_a.type() == _b.type() &&
_a.name() == _b.name() &&
math::equal(
@@ -295,6 +327,19 @@ class PoseCommand : public UserCommandBase
public: bool Execute() final;
};
+/// \brief Command to update an entity's pose transform.
+class PoseVectorCommand : public UserCommandBase
+{
+ /// \brief Constructor
+ /// \param[in] _msg pose_v message.
+ /// \param[in] _iface Pointer to user commands interface.
+ public: PoseVectorCommand(msgs::Pose_V *_msg,
+ std::shared_ptr &_iface);
+
+ // Documentation inherited
+ public: bool Execute() final;
+};
+
/// \brief Command to modify the physics parameters of a simulation.
class PhysicsCommand : public UserCommandBase
{
@@ -454,6 +499,13 @@ class ignition::gazebo::systems::UserCommandsPrivate
/// \return True if successful.
public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res);
+ /// \brief Callback for pose_v service
+ /// \param[in] _req Request containing pose update of several entities.
+ /// \param[out] _res True if message successfully received and queued.
+ /// It does not mean that the entity will be successfully moved.
+ /// \return True if successful.
+ public: bool PoseVectorService(const msgs::Pose_V &_req, msgs::Boolean &_res);
+
/// \brief Callback for physics service
/// \param[in] _req Request containing updates to the physics parameters.
/// \param[out] _res True if message successfully received and queued.
@@ -505,6 +557,26 @@ class ignition::gazebo::systems::UserCommandsPrivate
public: std::mutex pendingMutex;
};
+/// \brief Pose3d equality comparison function.
+/// \param[in] _a A pose to compare
+/// \param[in] _b Another pose to compare
+bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b)
+{
+ return _a.Pos().Equal(_b.Pos(), 1e-6) &&
+ math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) &&
+ math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) &&
+ math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) &&
+ math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6);
+}
+
+/// \brief Update pose for a specific pose message
+/// \param[in] _req Message containing new pose
+/// \param[in] _iface Pointer to user commands interface.
+/// \return True if successful.
+bool updatePose(
+ const msgs::Pose &_req,
+ std::shared_ptr _iface);
+
//////////////////////////////////////////////////
UserCommands::UserCommands() : System(),
dataPtr(std::make_unique())
@@ -598,6 +670,14 @@ void UserCommands::Configure(const Entity &_entity,
ignmsg << "Pose service on [" << poseService << "]" << std::endl;
+ // Pose vector service
+ std::string poseVectorService{
+ "/world/" + worldName + "/set_pose_vector"};
+ this->dataPtr->node.Advertise(poseVectorService,
+ &UserCommandsPrivate::PoseVectorService, this->dataPtr.get());
+
+ ignmsg << "Pose service on [" << poseVectorService << "]" << std::endl;
+
// Light service
std::string lightService{"/world/" + validWorldName + "/light_config"};
this->dataPtr->node.Advertise(lightService,
@@ -796,6 +876,25 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req,
return true;
}
+//////////////////////////////////////////////////
+bool UserCommandsPrivate::PoseVectorService(const msgs::Pose_V &_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::EnableCollisionService(const msgs::Entity &_req,
msgs::Boolean &_res)
@@ -1302,59 +1401,94 @@ bool LightCommand::Execute()
}
//////////////////////////////////////////////////
-PoseCommand::PoseCommand(msgs::Pose *_msg,
- std::shared_ptr &_iface)
- : UserCommandBase(_msg, _iface)
-{
-}
-
-//////////////////////////////////////////////////
-bool PoseCommand::Execute()
+bool updatePose(
+ const msgs::Pose &_poseMsg,
+ std::shared_ptr _iface)
{
- auto poseMsg = dynamic_cast(this->msg);
- if (nullptr == poseMsg)
- {
- ignerr << "Internal error, null create message" << std::endl;
- return false;
- }
-
// Check the name of the entity being spawned
- std::string entityName = poseMsg->name();
+ std::string entityName = _poseMsg.name();
Entity entity = kNullEntity;
// TODO(anyone) Update pose message to use Entity, with default ID null
- if (poseMsg->id() != kNullEntity && poseMsg->id() != 0)
+ if (_poseMsg.id() != kNullEntity && _poseMsg.id() != 0)
{
- entity = poseMsg->id();
+ entity = _poseMsg.id();
}
else if (!entityName.empty())
{
- entity = this->iface->ecm->EntityByComponents(components::Name(entityName),
- components::ParentEntity(this->iface->worldEntity));
+ entity = _iface->ecm->EntityByComponents(components::Name(entityName),
+ components::ParentEntity(_iface->worldEntity));
}
- if (!this->iface->ecm->HasEntity(entity))
+ if (!_iface->ecm->HasEntity(entity))
{
- ignerr << "Unable to update the pose for entity id:[" << poseMsg->id()
+ ignerr << "Unable to update the pose for entity id:[" << _poseMsg.id()
<< "], name[" << entityName << "]" << std::endl;
return false;
}
auto poseCmdComp =
- this->iface->ecm->Component(entity);
+ _iface->ecm->Component(entity);
if (!poseCmdComp)
{
- this->iface->ecm->CreateComponent(
- entity, components::WorldPoseCmd(msgs::Convert(*poseMsg)));
+ _iface->ecm->CreateComponent(
+ entity, components::WorldPoseCmd(msgs::Convert(_poseMsg)));
}
else
{
/// \todo(anyone) Moving an object is not captured in a log file.
- auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), pose3Eql) ?
+ auto state = poseCmdComp->SetData(msgs::Convert(_poseMsg), pose3Eql) ?
ComponentState::OneTimeChange :
ComponentState::NoChange;
- this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
+ _iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId,
state);
}
+ return true;
+}
+
+//////////////////////////////////////////////////
+PoseCommand::PoseCommand(msgs::Pose *_msg,
+ std::shared_ptr &_iface)
+ : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseCommand::Execute()
+{
+ auto poseMsg = dynamic_cast(this->msg);
+ if (nullptr == poseMsg)
+ {
+ ignerr << "Internal error, null create message" << std::endl;
+ return false;
+ }
+
+ return updatePose(*poseMsg, this->iface);
+}
+
+//////////////////////////////////////////////////
+PoseVectorCommand::PoseVectorCommand(msgs::Pose_V *_msg,
+ std::shared_ptr &_iface)
+ : UserCommandBase(_msg, _iface)
+{
+}
+
+//////////////////////////////////////////////////
+bool PoseVectorCommand::Execute()
+{
+ auto poseVectorMsg = dynamic_cast(this->msg);
+ if (nullptr == poseVectorMsg)
+ {
+ ignerr << "Internal error, null create message" << std::endl;
+ return false;
+ }
+
+ for (int i = 0; i < poseVectorMsg->pose_size(); i++)
+ {
+ if (!updatePose(poseVectorMsg->pose(i), this->iface))
+ {
+ return false;
+ }
+ }
return true;
}
diff --git a/src/systems/user_commands/UserCommands.hh b/src/systems/user_commands/UserCommands.hh
index 3db285aad6..95af084c96 100644
--- a/src/systems/user_commands/UserCommands.hh
+++ b/src/systems/user_commands/UserCommands.hh
@@ -53,6 +53,22 @@ namespace systems
/// * **Request type*: ignition.msgs.EntityFactory_V
/// * **Response type*: ignition.msgs.Boolean
///
+ /// # Set entity pose
+ ///
+ /// This service set the pose of entities
+ ///
+ /// * **Service**: `/world//set_pose`
+ /// * **Request type*: ignition.msgs.Pose
+ /// * **Response type*: ignition.msgs.Boolean
+ ///
+ /// # Set multiple entity poses
+ ///
+ /// This service set the pose of multiple entities
+ ///
+ /// * **Service**: `/world//set_pose_vector`
+ /// * **Request type*: ignition.msgs.Pose_V
+ /// * **Response type*: ignition.msgs.Boolean
+ ///
/// Try some examples described on examples/worlds/empty.sdf
class UserCommands:
public System,
diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc
index 18f4d4a432..48c9a18bc8 100644
--- a/test/integration/user_commands.cc
+++ b/test/integration/user_commands.cc
@@ -690,6 +690,81 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(Pose))
EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2);
}
+
+/////////////////////////////////////////////////
+TEST_F(UserCommandsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(PoseVector))
+{
+ // Start server
+ ServerConfig serverConfig;
+ const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+ "/test/worlds/shapes.sdf";
+ serverConfig.SetSdfFile(sdfFile);
+
+ Server server(serverConfig);
+ EXPECT_FALSE(server.Running());
+ EXPECT_FALSE(*server.Running(0));
+
+ // Create a system just to get the ECM
+ EntityComponentManager *ecm{nullptr};
+ test::Relay testSystem;
+ testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &,
+ gazebo::EntityComponentManager &_ecm)
+ {
+ ecm = &_ecm;
+ });
+
+ server.AddSystem(testSystem.systemPtr);
+
+ // Run server and check we have the ECM
+ EXPECT_EQ(nullptr, ecm);
+ server.Run(true, 1, false);
+ EXPECT_NE(nullptr, ecm);
+
+ // Entity move by name
+ msgs::Pose_V req;
+
+ auto poseBoxMsg = req.add_pose();
+ poseBoxMsg->set_name("box");
+ poseBoxMsg->mutable_position()->set_y(123.0);
+
+ auto poseSphereMsg = req.add_pose();
+ poseSphereMsg->set_name("sphere");
+ poseSphereMsg->mutable_position()->set_y(456.0);
+
+ msgs::Boolean res;
+ bool result;
+ unsigned int timeout = 5000;
+ std::string service{"/world/default/set_pose_vector"};
+
+ transport::Node node;
+ EXPECT_TRUE(node.Request(service, req, timeout, res, result));
+ EXPECT_TRUE(result);
+ EXPECT_TRUE(res.data());
+
+ // Box entity
+ auto boxEntity = ecm->EntityByComponents(components::Name("box"));
+ EXPECT_NE(kNullEntity, boxEntity);
+
+ // Check entity has not been moved yet
+ auto poseComp = ecm->Component(boxEntity);
+ ASSERT_NE(nullptr, poseComp);
+ EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 1), poseComp->Data());
+
+ // Run an iteration and check it was moved
+ server.Run(true, 1, false);
+
+ poseComp = ecm->Component(boxEntity);
+ ASSERT_NE(nullptr, poseComp);
+ EXPECT_NEAR(123.0, poseComp->Data().Pos().Y(), 0.2);
+
+ auto sphereEntity = ecm->EntityByComponents(components::Name("sphere"));
+ EXPECT_NE(kNullEntity, sphereEntity);
+
+ poseComp = ecm->Component(sphereEntity);
+ ASSERT_NE(nullptr, poseComp);
+ EXPECT_NEAR(456, poseComp->Data().Pos().Y(), 0.2);
+}
+
/////////////////////////////////////////////////
// https://github.com/ignitionrobotics/ign-gazebo/issues/634
TEST_F(UserCommandsTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light))
@@ -758,6 +833,14 @@ TEST_F(UserCommandsTest, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Light))
req.set_attenuation_constant(0.6f);
req.set_attenuation_quadratic(0.001f);
req.set_cast_shadows(true);
+
+ // todo(ahcorde) Use the field is_light_off in light.proto from
+ // Garden on.
+ auto header = req.mutable_header()->add_data();
+ header->set_key("isLightOn");
+ std::string *value = header->add_value();
+ *value = std::to_string(true);
+
EXPECT_TRUE(node.Request(service, req, timeout, res, result));
EXPECT_TRUE(result);
EXPECT_TRUE(res.data());