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());