diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md new file mode 100644 index 0000000000..8e30646e38 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -0,0 +1,72 @@ +--- +name: Bug report +about: Report a bug +labels: bug +--- + + + +## Environment +* OS Version: +* Source or binary build? + + +* If this is a GUI or sensor rendering bug, describe your GPU and rendering system. Otherwise delete this section. + - Rendering plugin: [ogre | ogre2]. + - [ ] Sensor rendering error. + - [ ] GUI rendering error. + - EGL headless mode: + - [ ] Running in EGL headless mode + - Generally, mention all circumstances that might affect rendering capabilities: + - [ ] running on a dual GPU machine (integrated GPU + discrete GPU) + - [ ] running on a multi-GPU machine (it has multiple discrete GPUs) + - [ ] running on real hardware + - [ ] running in virtual machine + - [ ] running in Docker/Singularity + - [ ] running remotely (e.g. via SSH) + - [ ] running in a cloud + - [ ] using VirtualGL, XVFB, Xdummy, XVNC or other indirect rendering utilities + - [ ] GPU is concurrently used for other tasks + - [ ] desktop acceleration + - [ ] video decoding (i.e. a playing Youtube video) + - [ ] video encoding + - [ ] CUDA/ROCm computations (Tensorflow, Torch, Caffe running) + - [ ] multiple simulators running at the same time + - [ ] other... + - Rendering system info: + - On Linux, provide the outputs of the following commands: + ```bash + LANG=C lspci -nn | grep VGA # might require installing pciutils + echo "$DISPLAY" + LANG=C glxinfo -B | grep -i '\(direct rendering\|opengl\|profile\)' # might require installing mesa-utils package + ps aux | grep Xorg + sudo env LANG=C X -version # if you don't have root access, try to tell the version of Xorg e.g. via package manager + ``` + - On Windows, run `dxdiag` and report the GPU-related information. + - On Mac OS, open a terminal and type `system_profiler SPDisplaysDataType`. Copy the output here. + + - [ ] Please, attach the ogre.log or ogre2.log file from `~/.ignition/rendering` + +
+ +``` +# paste log here +``` + +
+ +## Description +* Expected behavior: +* Actual behavior: + +## Steps to reproduce + + +1. +2. +3. + +## Output + diff --git a/.github/ISSUE_TEMPLATE/feature_request.md b/.github/ISSUE_TEMPLATE/feature_request.md new file mode 100644 index 0000000000..87233a479a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/feature_request.md @@ -0,0 +1,23 @@ +--- +name: Feature request +about: Request a new feature +labels: enhancement +--- + + + +## Desired behavior + + +## Alternatives considered + + +## Implementation suggestion + + +## Additional context + diff --git a/README.md b/README.md index 9bc1ff623b..a315f2df4b 100644 --- a/README.md +++ b/README.md @@ -123,7 +123,7 @@ export GZ_CONFIG_PATH=$HOME/.gz/tools/configs This issue is tracked [here](https://github.com/gazebosim/gz-tools/issues/8). -# Documentation +# Documentation See the [installation tutorial](https://gazebosim.org/api/gazebo/6.1/install.html). diff --git a/examples/scripts/python_api/README.md b/examples/scripts/python_api/README.md index fdf9b6b212..43d1abe67e 100644 --- a/examples/scripts/python_api/README.md +++ b/examples/scripts/python_api/README.md @@ -3,4 +3,4 @@ This example shows how to use Gazebo Sim's Python API. For more information, see the -[Python interfaces](https://gazebosim.org/api/gazebo/6.4/python_interfaces.html) tutorial. +[Python interfaces](https://gazebosim.org/api/gazebo/6.9/python_interfaces.html) tutorial. diff --git a/examples/scripts/python_api/testFixture.py b/examples/scripts/python_api/testFixture.py index 1740934366..f2c0e63af4 100644 --- a/examples/scripts/python_api/testFixture.py +++ b/examples/scripts/python_api/testFixture.py @@ -23,7 +23,6 @@ # python3 examples/scripts/python_api/testFixture.py import os -import time from ignition.common import set_verbosity from ignition.gazebo import TestFixture, World, world_entity @@ -47,7 +46,7 @@ def on_pre_udpate_cb(_info, _ecm): pre_iterations += 1 if first_iteration: first_iteration = False - world_e = world_entity(_ecm); + world_e = world_entity(_ecm) print('World entity is ', world_e) w = World(world_e) v = w.gravity(_ecm) @@ -74,10 +73,7 @@ def on_post_udpate_cb(_info, _ecm): fixture.finalize() server = fixture.server() -server.run(False, 1000, False) - -while(server.is_running()): - time.sleep(0.1) +server.run(True, 1000, False) print('iterations ', iterations) print('post_iterations ', post_iterations) diff --git a/examples/worlds/multi_lrauv_race.sdf b/examples/worlds/multi_lrauv_race.sdf index d85eb31c10..39fed280ac 100644 --- a/examples/worlds/multi_lrauv_race.sdf +++ b/examples/worlds/multi_lrauv_race.sdf @@ -59,7 +59,7 @@ -5 0 0 0 0 0 - https://fuel.ignitionrobotics.org/1.0/mabel/models/Turquoise turbidity generator + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/Turquoise turbidity generator @@ -346,12 +346,12 @@ 0 0 -1 0 0 3.1415926 - https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m start_line 0 -25 -1 0 0 3.1415926 - https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + https://fuel.ignitionrobotics.org/1.0/mabelzhang/models/ABCSign_5m finish_line diff --git a/examples/worlds/polylines.sdf b/examples/worlds/polylines.sdf new file mode 100644 index 0000000000..5178eb54ca --- /dev/null +++ b/examples/worlds/polylines.sdf @@ -0,0 +1,233 @@ + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + -5 0 5 0 1.57 0 + + + 0 0 0.5 0 0 0 + 1.0 + + 0.1666 + 0 + 0 + 0.1666 + 0 + 0.1666 + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + + + + + -0.5 -0.5 + -0.5 0.5 + 0.5 0.5 + 0 0 + 0.5 -0.5 + 1 + + + + 1.0 0 0 1 + 1.0 0 0 1 + 1.0 0 0 1 + + + + + + + -5 -3 5 0 1.3 0 + + + 0 0 2 0 0 0 + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + + + + + -0.3 0.5 + 0.3 0.3 + 0.3 -0.3 + -0.3 -0.5 + -0.5 0 + 4 + + + + 0 1.0 0 1 + 0 1.0 0 1 + 0 1.0 0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 0.5 0.5 0.75 0 0 0 + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + + + + + 0 0 + 0 1 + 1 1 + 1 0 + 1.5 + + + + 0 1.0 1.0 1 + 0 1.0 1.0 1 + 0 1.0 1.0 1 + + + + + + + -5 2 5 0 1.57 0 + + + 2.2 1.5 0.5 0 0 0 + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + + + + + 2.27467 1.0967 + 1.81094 2.35418 + 2.74009 2.35418 + 1.0 + + + 2.08173 0.7599 + 2.4693 0.7599 + 3.4323 3.28672 + 3.07689 3.28672 + 2.84672 2.63851 + 1.7077 2.63851 + 1.47753 3.28672 + 1.11704 3.28672 + 1.0 + + + + 1.0 0 1.0 1 + 1.0 0 1.0 1 + 1.0 0 1.0 1 + + + + + + diff --git a/examples/worlds/triggered_camera_sensor.sdf b/examples/worlds/triggered_camera_sensor.sdf new file mode 100644 index 0000000000..1b7d450934 --- /dev/null +++ b/examples/worlds/triggered_camera_sensor.sdf @@ -0,0 +1,323 @@ + + + + + + 0.001 + 1.0 + + + + + ogre + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + true + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + docked + + + + + + + docked + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + true + 3 0 0.5 0 0 0 + + + + + 0.125 + + + + + + + 0.125 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + false + + + + + + 4 0 1.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + true + camera/trigger + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + true + camera + + + + + + 0 1 3 0.0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + diff --git a/include/gz/sim/Conversions.hh b/include/gz/sim/Conversions.hh index 5f55f4e3a6..1deb39a5a6 100644 --- a/include/gz/sim/Conversions.hh +++ b/include/gz/sim/Conversions.hh @@ -27,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -49,6 +50,7 @@ #include #include #include +#include #include #include #include @@ -709,6 +711,38 @@ namespace gz /// \return Particle emitter message. template<> sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); + + /// \brief Generic conversion from an SDF element to another type. + /// \param[in] _in SDF element. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Element &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF element to a plugin message. + /// \param[in] _in SDF element. + /// \return Plugin message. + template<> + msgs::Plugin convert(const sdf::Element &_in); + + /// \brief Generic conversion from an SDF plugin to another type. + /// \param[in] _in SDF plugin. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Plugin &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF plugin to a plugin message. + /// \param[in] _in SDF plugin. + /// \return Plugin message. + template<> + msgs::Plugin convert(const sdf::Plugin &_in); } } } diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index 6ec0cd844e..af6fc57647 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -172,7 +172,8 @@ namespace gz /// Setting the SDF file will override any value set by `SetSdfString`. /// /// \param[in] _file Full path to an SDF file. - /// \return (reserved for future use) + /// \return True if the file was set, false if the file was not set. + /// The file will not be set if the provide _file string is empty. public: bool SetSdfFile(const std::string &_file); /// \brief Get the SDF file that has been set. An empty string will be diff --git a/include/gz/sim/components/Model.hh b/include/gz/sim/components/Model.hh index 403b40aa5d..6aa829ab8d 100644 --- a/include/gz/sim/components/Model.hh +++ b/include/gz/sim/components/Model.hh @@ -46,13 +46,38 @@ namespace serializers sdf::ElementPtr modelElem = _model.Element(); if (!modelElem) { - gzerr << "Unable to serialize sdf::Model" << std::endl; + gzwarn << "Unable to serialize sdf::Model" << std::endl; return _out; } + bool skip = false; + if (modelElem->HasElement("pose")) + { + sdf::ElementPtr poseElem = modelElem->GetElement("pose"); + if (poseElem->HasAttribute("relative_to")) + { + // Skip serializing models with //pose/@relative_to attribute + // since deserialization will fail. This could be a nested model. + // see https://github.com/ignitionrobotics/ign-gazebo/issues/1071 + // Once https://github.com/ignitionrobotics/sdformat/issues/820 is + // resolved, there should be an API that returns sdf::Errors objects + // instead of printing console msgs so it would be easier to ignore + // specific errors in Deserialize. + static bool warned = false; + if (!warned) + { + gzwarn << "Skipping serialization / deserialization for models " + << "with //pose/@relative_to attribute." + << std::endl; + warned = true; + } + skip = true; + } + } + _out << "" << "" - << modelElem->ToString("") + << (skip ? std::string() : modelElem->ToString("")) << ""; return _out; } @@ -70,7 +95,7 @@ namespace serializers sdf::Errors errors = root.LoadSdfString(sdf); if (!root.Model()) { - gzerr << "Unable to unserialize sdf::Model" << std::endl; + gzwarn << "Unable to deserialize sdf::Model" << std::endl; return _in; } diff --git a/include/gz/sim/components/SystemPluginInfo.hh b/include/gz/sim/components/SystemPluginInfo.hh new file mode 100644 index 0000000000..4c05317fa5 --- /dev/null +++ b/include/gz/sim/components/SystemPluginInfo.hh @@ -0,0 +1,47 @@ +/* + * 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 GZ_SIM_COMPONENTS_SYSTEMINFO_HH_ +#define GZ_SIM_COMPONENTS_SYSTEMINFO_HH_ + +#include +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief This component holds information about all the system plugins that + /// are attached to an entity. The content of each system is populated the + /// moment the plugin is instantiated and isn't modified throughout + /// simulation. + using SystemPluginInfo = Component; + IGN_GAZEBO_REGISTER_COMPONENT("gz_sim_components.SystemPluginInfo", + SystemPluginInfo) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/SystemPluginInfo.hh b/include/ignition/gazebo/components/SystemPluginInfo.hh new file mode 100644 index 0000000000..1fd4e3d103 --- /dev/null +++ b/include/ignition/gazebo/components/SystemPluginInfo.hh @@ -0,0 +1,19 @@ +/* + * 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. + * + */ + +#include +#include diff --git a/python/src/gz/sim/Server.cc b/python/src/gz/sim/Server.cc index 1b84654a98..e0f0330174 100644 --- a/python/src/gz/sim/Server.cc +++ b/python/src/gz/sim/Server.cc @@ -35,6 +35,7 @@ void defineGazeboServer(pybind11::object module) .def(pybind11::init()) .def( "run", &gz::sim::Server::Run, + pybind11::call_guard(), "Run the server. By default this is a non-blocking call, " " which means the server runs simulation in a separate thread. Pass " " in true to the _blocking argument to run the server in the current " diff --git a/python/test/testFixture_TEST.py b/python/test/testFixture_TEST.py index 8a0d9995dd..f2ffcc1043 100644 --- a/python/test/testFixture_TEST.py +++ b/python/test/testFixture_TEST.py @@ -13,7 +13,6 @@ # limitations under the License. import os -import time import unittest from ignition.common import set_verbosity @@ -39,7 +38,7 @@ def on_post_udpate_cb(_info, _ecm): def on_pre_udpate_cb(_info, _ecm): global pre_iterations pre_iterations += 1 - world_e = world_entity(_ecm); + world_e = world_entity(_ecm) self.assertEqual(1, world_e) w = World(world_e) v = w.gravity(_ecm) @@ -55,10 +54,7 @@ def on_udpate_cb(_info, _ecm): fixture.finalize() server = fixture.server() - server.run(False, 1000, False) - - while(server.is_running()): - time.sleep(0.1) + server.run(True, 1000, False) self.assertEqual(1000, pre_iterations) self.assertEqual(1000, iterations) diff --git a/src/Conversions.cc b/src/Conversions.cc index b4c0e1136c..5da23fdf70 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -64,6 +64,7 @@ #include #include #include +#include #include #include @@ -242,6 +243,20 @@ msgs::Geometry gz::sim::convert(const sdf::Geometry &_in) blendMsg->set_fade_dist(blendSdf->FadeDistance()); } } + else if (_in.Type() == sdf::GeometryType::POLYLINE && + !_in.PolylineShape().empty()) + { + out.set_type(msgs::Geometry::POLYLINE); + for (const auto &polyline : _in.PolylineShape()) + { + auto polylineMsg = out.add_polyline(); + polylineMsg->set_height(polyline.Height()); + for (const auto &point : polyline.Points()) + { + msgs::Set(polylineMsg->add_point(), point); + } + } + } else { gzerr << "Geometry type [" << static_cast(_in.Type()) @@ -357,6 +372,28 @@ sdf::Geometry gz::sim::convert(const msgs::Geometry &_in) out.SetHeightmapShape(heightmapShape); } + else if (_in.type() == msgs::Geometry::POLYLINE && _in.polyline_size() > 0) + { + out.SetType(sdf::GeometryType::POLYLINE); + + std::vector polylines; + + for (auto i = 0; i < _in.polyline().size(); ++i) + { + auto polylineMsg = _in.polyline(i); + sdf::Polyline polylineShape; + polylineShape.SetHeight(polylineMsg.height()); + + for (auto j = 0; j < polylineMsg.point().size(); ++j) + { + polylineShape.AddPoint(msgs::Convert(polylineMsg.point(j))); + } + + polylines.push_back(polylineShape); + } + + out.SetPolylineShape(polylines); + } else { gzerr << "Geometry type [" << static_cast(_in.type()) @@ -624,28 +661,13 @@ msgs::GUI gz::sim::convert(const sdf::Gui &_in) out.set_fullscreen(_in.Fullscreen()); // Set gui plugins - auto elem = _in.Element(); - if (elem && elem->HasElement("plugin")) + for (uint64_t i = 0; i < _in.PluginCount(); ++i) { - auto pluginElem = elem->GetElement("plugin"); - while (pluginElem) - { - auto pluginMsg = out.add_plugin(); - pluginMsg->set_name(pluginElem->Get("name")); - pluginMsg->set_filename(pluginElem->Get("filename")); - - std::stringstream ss; - for (auto innerElem = pluginElem->GetFirstElement(); - innerElem; innerElem = innerElem->GetNextElement("")) - { - ss << innerElem->ToString(""); - } - pluginMsg->set_innerxml(ss.str()); - pluginElem = pluginElem->GetNextElement("plugin"); - } + auto pluginMsg = out.add_plugin(); + pluginMsg->CopyFrom(convert(*_in.PluginByIndex(i))); } - if (elem->HasElement("camera")) + if (_in.Element()->HasElement("camera")) { gzwarn << " can't be converted yet" << std::endl; } @@ -1672,3 +1694,51 @@ sdf::ParticleEmitter gz::sim::convert(const msgs::ParticleEmitter &_in) return out; } + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Plugin gz::sim::convert(const sdf::Element &_in) +{ + msgs::Plugin result; + + if (_in.GetName() != "plugin") + { + gzerr << "Tried to convert SDF [" << _in.GetName() + << "] into [plugin]" << std::endl; + return result; + } + + result.set_name(_in.Get("name")); + result.set_filename(_in.Get("filename")); + + std::stringstream ss; + for (auto innerElem = _in.GetFirstElement(); innerElem; + innerElem = innerElem->GetNextElement("")) + { + ss << innerElem->ToString(""); + } + result.set_innerxml(ss.str()); + + return result; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Plugin gz::sim::convert(const sdf::Plugin &_in) +{ + msgs::Plugin result; + + result.set_name(_in.Name()); + result.set_filename(_in.Filename()); + + std::stringstream ss; + for (auto content : _in.Contents()) + { + ss << content->ToString(""); + } + result.set_innerxml(ss.str()); + + return result; +} diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index d0dae828ad..e559cd33b3 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include #include #include +#include #include #include #include @@ -569,6 +571,40 @@ TEST(Conversions, GeometryHeightmap) EXPECT_DOUBLE_EQ(456.789, newHeightmap->BlendByIndex(0)->FadeDistance()); } +///////////////////////////////////////////////// +TEST(Conversions, GeometryPolyline) +{ + sdf::Geometry geometry; + geometry.SetType(sdf::GeometryType::POLYLINE); + + sdf::Polyline polylineShape; + polylineShape.SetHeight(1.23); + polylineShape.AddPoint({4.5, 6.7}); + polylineShape.AddPoint({8.9, 10.11}); + geometry.SetPolylineShape({polylineShape}); + + auto geometryMsg = convert(geometry); + EXPECT_EQ(msgs::Geometry::POLYLINE, geometryMsg.type()); + ASSERT_EQ(1, geometryMsg.polyline_size()); + EXPECT_DOUBLE_EQ(1.23, geometryMsg.polyline(0).height()); + ASSERT_EQ(2, geometryMsg.polyline(0).point_size()); + EXPECT_DOUBLE_EQ(4.5, geometryMsg.polyline(0).point(0).x()); + EXPECT_DOUBLE_EQ(6.7, geometryMsg.polyline(0).point(0).y()); + EXPECT_DOUBLE_EQ(8.9, geometryMsg.polyline(0).point(1).x()); + EXPECT_DOUBLE_EQ(10.11, geometryMsg.polyline(0).point(1).y()); + + auto newGeometry = convert(geometryMsg); + EXPECT_EQ(sdf::GeometryType::POLYLINE, newGeometry.Type()); + ASSERT_FALSE(newGeometry.PolylineShape().empty()); + ASSERT_EQ(1u, newGeometry.PolylineShape().size()); + EXPECT_DOUBLE_EQ(1.23, newGeometry.PolylineShape()[0].Height()); + ASSERT_EQ(2u, newGeometry.PolylineShape()[0].PointCount()); + EXPECT_DOUBLE_EQ(4.5, newGeometry.PolylineShape()[0].PointByIndex(0)->X()); + EXPECT_DOUBLE_EQ(6.7, newGeometry.PolylineShape()[0].PointByIndex(0)->Y()); + EXPECT_DOUBLE_EQ(8.9, newGeometry.PolylineShape()[0].PointByIndex(1)->X()); + EXPECT_DOUBLE_EQ(10.11, newGeometry.PolylineShape()[0].PointByIndex(1)->Y()); +} + ///////////////////////////////////////////////// TEST(Conversions, Inertial) { @@ -725,7 +761,7 @@ TEST(Conversions, Atmosphere) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, MagnetometerSensor) +TEST(Conversions, MagnetometerSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); @@ -765,7 +801,7 @@ TEST(CONVERSIONS, MagnetometerSensor) } ///////////////////////////////////////////////// -TEST(CONVERSIONS, AltimeterSensor) +TEST(Conversions, AltimeterSensor) { sdf::Sensor sensor; sensor.SetName("my_sensor"); @@ -1030,3 +1066,51 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ(emitter2.RawPose(), emitter.RawPose()); EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio()); } + +///////////////////////////////////////////////// +TEST(Conversions, PluginElement) +{ + sdf::Root root; + root.LoadSdfString("" + "" + " " + " 0.5" + " " + ""); + + auto world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + auto worldElem = world->Element(); + ASSERT_NE(nullptr, worldElem); + + auto pluginElem = worldElem->GetElement("plugin"); + ASSERT_NE(nullptr, pluginElem); + + auto pluginMsg = convert(*(pluginElem.get())); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos); +} + +///////////////////////////////////////////////// +TEST(Conversions, Plugin) +{ + sdf::Plugin pluginSdf; + pluginSdf.SetName("peach"); + pluginSdf.SetFilename("plum"); + + auto content = std::make_shared(); + content->SetName("avocado"); + content->AddValue("double", "0.5", false, ""); + pluginSdf.InsertContent(content); + + auto pluginMsg = convert(pluginSdf); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos) << pluginMsg.innerxml(); +} diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 60fb5cff65..9d770b1567 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -356,6 +356,21 @@ void ServerPrivate::SetupTransport() << "]" << std::endl; } + // Advertise a service that returns the full path, on the Gazebo server's + // host machine, based on a provided URI. + std::string resolvePathService{"/gazebo/resource_paths/resolve"}; + if (this->node.Advertise(resolvePathService, + &ServerPrivate::ResourcePathsResolveService, this)) + { + gzmsg << "Resource path resolve service on [" << resolvePathService << "]." + << std::endl; + } + else + { + gzerr << "Something went wrong, failed to advertise [" << getPathService + << "]" << std::endl; + } + std::string pathTopic{"/gazebo/resource_paths"}; this->pathPub = this->node.Advertise(pathTopic); @@ -485,6 +500,64 @@ bool ServerPrivate::ResourcePathsService( return true; } +////////////////////////////////////////////////// +bool ServerPrivate::ResourcePathsResolveService( + const msgs::StringMsg &_req, + msgs::StringMsg &_res) +{ + // Get the request + std::string req = _req.data(); + + // Handle the case where the request is already a valid path + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + + // Try Fuel + std::string path = + fuel_tools::fetchResourceWithClient(req, *this->fuelClient.get()); + if (!path.empty() && common::exists(path)) + { + _res.set_data(path); + return true; + } + + // Check for the file:// prefix. + std::string prefix = "file://"; + if (req.find(prefix) == 0) + { + req = req.substr(prefix.size()); + // Check to see if the path exists + if (common::exists(req)) + { + _res.set_data(req); + return true; + } + } + + // Check for the model:// prefix + prefix = "model://"; + if (req.find(prefix) == 0) + req = req.substr(prefix.size()); + + // Checkout resource paths + std::vector gzPaths = resourcePaths(); + for (const std::string &gzPath : gzPaths) + { + std::string fullPath = common::joinPaths(gzPath, req); + if (common::exists(fullPath)) + { + _res.set_data(fullPath); + return true; + } + } + + // Otherwise the resource could not be found + return false; +} + ////////////////////////////////////////////////// std::string ServerPrivate::FetchResource(const std::string &_uri) { diff --git a/src/ServerPrivate.hh b/src/ServerPrivate.hh index 08e83cdb17..adb0f8bd76 100644 --- a/src/ServerPrivate.hh +++ b/src/ServerPrivate.hh @@ -117,6 +117,28 @@ namespace gz /// \return True if successful. private: bool ResourcePathsService(gz::msgs::StringMsg_V &_res); + /// \brief Callback for a resource path resolve service. This service + /// will return the full path to a provided resource's URI. An empty + /// string and return value of false will be used if the resource could + /// not be found. + /// + /// Fuel will be checked and then the GZ_GAZEBO_RESOURCE_PATH environment + /// variable paths. This service will not check for files relative to + /// working directory of the Gazebo server. + /// + /// \param[in] _req Request filled with a resource URI to resolve. + /// Example values could be: + /// * https://URI_TO_A_FUEL_RESOURCE + /// * model://MODLE_NAME/meshes/MESH_NAME + /// * file://PATH/TO/FILE + /// + /// \param[out] _res Response filled with the resolved path, or empty + /// if the resource could not be found. + /// \return True if successful, false otherwise. + private: bool ResourcePathsResolveService( + const msgs::StringMsg &_req, + msgs::StringMsg &_res); + /// \brief Callback for server control service. /// \param[out] _req The control request. /// \param[out] _res Whether the request was successfully fullfilled. diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index 35d50c69ba..953377470e 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1033,6 +1033,78 @@ TEST_P(ServerFixture, AddResourcePaths) } } +///////////////////////////////////////////////// +TEST_P(ServerFixture, ResolveResourcePaths) +{ + common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + common::setenv("SDF_PATH", ""); + common::setenv("IGN_FILE_PATH", ""); + + ServerConfig serverConfig; + Server server(serverConfig); + + EXPECT_FALSE(*server.Running(0)); + + auto test = std::function( + [&](const std::string &_uri, const std::string &_expected, bool _found) + { + transport::Node node; + msgs::StringMsg req, res; + bool result{false}; + bool executed{false}; + int sleep{0}; + int maxSleep{30}; + + req.set_data(_uri); + while (!executed && sleep < maxSleep) + { + gzdbg << "Requesting /gazebo/resource_paths/resolve" << std::endl; + executed = node.Request("/gazebo/resource_paths/resolve", req, 100, + res, result); + sleep++; + } + EXPECT_TRUE(executed); + EXPECT_EQ(_found, result); + EXPECT_EQ(_expected, res.data()) << "Expected[" << _expected + << "] Received[" << res.data() << "]"; + }); + + // Make sure the resource path is clear + common::setenv("IGN_GAZEBO_RESOURCE_PATH", ""); + + // An absolute path should return the same absolute path + test(PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // An absolute path, with the file:// prefix, should return the absolute path + test(std::string("file://") + + PROJECT_SOURCE_PATH, PROJECT_SOURCE_PATH, true); + + // A non-absolute path with no RESOURCE_PATH should not find the resource + test(common::joinPaths("test", "worlds", "plugins.sdf"), "", false); + + // Try again, this time with a RESOURCE_PATH + common::setenv("IGN_GAZEBO_RESOURCE_PATH", PROJECT_SOURCE_PATH); + test(common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + // With the file:// prefix should also work + test(std::string("file://") + + common::joinPaths("test", "worlds", "plugins.sdf"), + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "plugins.sdf"), + true); + + // The model:// URI should not resolve + test("model://include_nested/model.sdf", "", false); + common::setenv("IGN_GAZEBO_RESOURCE_PATH", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models")); + // The model:// URI should now resolve because the RESOURCE_PATH has been + // updated. + test("model://include_nested/model.sdf", + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models", + "include_nested", "model.sdf"), true); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 38327b139b..34a8a5da32 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -15,6 +15,8 @@ * */ +#include "gz/sim/components/SystemPluginInfo.hh" +#include "gz/sim/Conversions.hh" #include "SystemManager.hh" using namespace gz; @@ -123,6 +125,29 @@ void SystemManager::AddSystemImpl( SystemInternal _system, std::shared_ptr _sdf) { + // Add component + if (this->entityCompMgr && kNullEntity != _system.parentEntity) + { + msgs::Plugin_V systemInfoMsg; + auto systemInfoComp = + this->entityCompMgr->Component( + _system.parentEntity); + if (systemInfoComp) + { + systemInfoMsg = systemInfoComp->Data(); + } + if (_sdf) + { + auto pluginMsg = systemInfoMsg.add_plugins(); + pluginMsg->CopyFrom(convert(*_sdf.get())); + } + + this->entityCompMgr->SetComponentData( + _system.parentEntity, systemInfoMsg); + this->entityCompMgr->SetChanged(_system.parentEntity, + components::SystemPluginInfo::typeId); + } + // Configure the system, if necessary if (_system.configure && this->entityCompMgr && this->eventMgr) { @@ -169,16 +194,15 @@ const std::vector& SystemManager::SystemsPostUpdate() ////////////////////////////////////////////////// std::vector SystemManager::TotalByEntity(Entity _entity) { + auto checkEntity = [&](const SystemInternal &_system) + { + return _system.parentEntity == _entity; + }; + std::vector result; - for (auto system : this->systems) - { - if (system.parentEntity == _entity) - result.push_back(system); - } - for (auto system : this->pendingSystems) - { - if (system.parentEntity == _entity) - result.push_back(system); - } + std::copy_if(this->systems.begin(), this->systems.end(), + std::back_inserter(result), checkEntity); + std::copy_if(this->pendingSystems.begin(), this->pendingSystems.end(), + std::back_inserter(result), checkEntity); return result; } diff --git a/src/SystemManager.hh b/src/SystemManager.hh index b1a5147155..f5ea223fb1 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -116,13 +116,20 @@ namespace gz /// \return Vector of systems. public: std::vector TotalByEntity(Entity _entity); + /// \brief Implementation for AddSystem functions that takes an SDF + /// element. This calls the AddSystemImpl that accepts an SDF Plugin. + /// \param[in] _system Generic representation of a system. + /// \param[in] _sdf SDF element. + private: void AddSystemImpl(SystemInternal _system, + std::shared_ptr _sdf); + /// \brief Implementation for AddSystem functions. This only adds systems /// to a queue, the actual addition is performed by `AddSystemToRunner` at /// the appropriate time. /// \param[in] _system Generic representation of a system. /// \param[in] _sdf SDF received from AddSystem. private: void AddSystemImpl(SystemInternal _system, - std::shared_ptr _sdf); + const sdf::Plugin &_sdf); /// \brief All the systems. private: std::vector systems; diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index e6336d68af..32072ec83c 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -21,6 +21,7 @@ #include "gz/sim/System.hh" #include "gz/sim/SystemLoader.hh" #include "gz/sim/Types.hh" +#include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/test_config.hh" // NOLINT(build/include) #include "SystemManager.hh" @@ -202,3 +203,60 @@ TEST(SystemManager, AddSystemEcm) EXPECT_EQ(1u, systemMgr.SystemsPostUpdate().size()); } +///////////////////////////////////////////////// +TEST(SystemManager, AddSystemWithInfo) +{ + auto loader = std::make_shared(); + + EntityComponentManager ecm; + auto entity = ecm.CreateEntity(); + EXPECT_NE(kNullEntity, entity); + + auto eventManager = EventManager(); + + SystemManager systemMgr(loader, &ecm, &eventManager); + + // No element, no SystemPluginInfo component + auto configSystem = std::make_shared(); + systemMgr.AddSystem(configSystem, entity, nullptr); + + // Element becomes SystemPluginInfo component + auto pluginElem = std::make_shared(); + sdf::initFile("plugin.sdf", pluginElem); + sdf::readString("" + " " + " 0.5" + " " + "", pluginElem); + + auto updateSystem = std::make_shared(); + systemMgr.AddSystem(updateSystem, entity, pluginElem); + + int entityCount{0}; + ecm.Each( + [&](const Entity &_entity, + const components::SystemPluginInfo *_systemInfoComp) -> bool + { + EXPECT_EQ(entity, _entity); + + EXPECT_NE(nullptr, _systemInfoComp); + if (nullptr == _systemInfoComp) + return true; + + auto pluginsMsg = _systemInfoComp->Data(); + EXPECT_EQ(1, pluginsMsg.plugins().size()); + if (1u != pluginsMsg.plugins().size()) + return true; + + auto pluginMsg = pluginsMsg.plugins(0); + EXPECT_EQ("plum", pluginMsg.filename()); + EXPECT_EQ("peach", pluginMsg.name()); + EXPECT_NE(pluginMsg.innerxml().find("0.5"), + std::string::npos); + + entityCount++; + return true; + }); + EXPECT_EQ(1, entityCount); +} + diff --git a/src/cmd/ign.cc b/src/cmd/ign.cc deleted file mode 100644 index 080dcb0813..0000000000 --- a/src/cmd/ign.cc +++ /dev/null @@ -1,461 +0,0 @@ -/* - * Copyright (C) 2019 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "gz/sim/config.hh" -#include "gz/sim/Server.hh" -#include "gz/sim/ServerConfig.hh" -#include "gui/GuiRunner.hh" -#include "ign.hh" - -////////////////////////////////////////////////// -extern "C" char *ignitionGazeboVersion() -{ - return strdup(GZ_SIM_VERSION_FULL); -} - -////////////////////////////////////////////////// -extern "C" char *gazeboVersionHeader() -{ - return strdup(GZ_SIM_VERSION_HEADER); -} - -////////////////////////////////////////////////// -extern "C" void cmdVerbosity( - const char *_verbosity) -{ - gz::common::Console::SetVerbosity(std::atoi(_verbosity)); -} - -////////////////////////////////////////////////// -extern "C" const char *worldInstallDir() -{ - return GZ_SIM_WORLD_INSTALL_DIR; -} - -////////////////////////////////////////////////// -extern "C" int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, const char *_networkRole, - int _networkSecondaries, int _record, const char *_recordPath, - int _logOverwrite, const char *_playback, const char *_file) -{ - gz::sim::ServerConfig serverConfig; - - // Path for logs - std::string recordPathMod = serverConfig.LogRecordPath(); - - // Initialize console log - if ((_recordPath != nullptr && std::strlen(_recordPath) > 0) || _record > 0) - { - if (_playback != nullptr && std::strlen(_playback) > 0) - { - gzerr << "Both record and playback are specified. Only specify one.\n"; - return -1; - } - - serverConfig.SetUseLogRecord(true); - - // If a record path is specified - if (_recordPath != nullptr && std::strlen(_recordPath) > 0) - { - recordPathMod = std::string(_recordPath); - - // Check if path or compressed file with same prefix exists - if (gz::common::exists(recordPathMod)) - { - // Overwrite if flag specified - if (_logOverwrite > 0) - { - bool recordMsg = false; - // Remove files before initializing console log files on top of them - if (gz::common::exists(recordPathMod)) - { - recordMsg = true; - gz::common::removeAll(recordPathMod); - } - - // Create log file before printing any messages so they can be logged - gzLogInit(recordPathMod, "server_console.log"); - - if (recordMsg) - { - gzmsg << "Log path already exists on disk! Existing files will " - << "be overwritten." << std::endl; - gzmsg << "Removing existing path [" << recordPathMod << "]\n"; - } - } - // Otherwise rename to unique path - else - { - // Remove the separator at end of path - if (!std::string(1, recordPathMod.back()).compare( - gz::common::separator(""))) - { - recordPathMod = recordPathMod.substr(0, recordPathMod.length() - 1); - } - recordPathMod = gz::common::uniqueDirectoryPath(recordPathMod); - - gzLogInit(recordPathMod, "server_console.log"); - gzwarn << "Log path already exists on disk! " - << "Recording instead to [" << recordPathMod << "]" << std::endl; - } - } - else - { - gzLogInit(recordPathMod, "server_console.log"); - } - } - // Empty record path specified. Use default. - else - { - // Create log file before printing any messages so they can be logged - gzLogInit(recordPathMod, "server_console.log"); - gzmsg << "Recording states to default path [" << recordPathMod << "]" - << std::endl; - - serverConfig.SetLogRecordPath(recordPathMod); - } - } - else - { - gzLogInit(serverConfig.LogRecordPath(), "server_console.log"); - } - - serverConfig.SetLogRecordPath(recordPathMod); - - gzmsg << "Ignition Gazebo Server v" << GZ_SIM_VERSION_FULL - << std::endl; - - // Set the SDF string to user - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - if (!serverConfig.SetSdfString(_sdfString)) - { - gzerr << "Failed to set SDF string [" << _sdfString << "]" << std::endl; - return -1; - } - } - serverConfig.SetSdfFile(_file); - - // Set the update rate. - if (_hz > 0.0) - serverConfig.SetUpdateRate(_hz); - - // Set whether levels should be used. - if (_levels > 0) - { - gzmsg << "Using the level system\n"; - serverConfig.SetUseLevels(true); - } - - if (_networkRole && std::strlen(_networkRole) > 0) - { - gzmsg << "Using the distributed simulation and levels systems\n"; - serverConfig.SetNetworkRole(_networkRole); - serverConfig.SetNetworkSecondaries(_networkSecondaries); - serverConfig.SetUseLevels(true); - } - - if (_playback != nullptr && std::strlen(_playback) > 0) - { - if (_sdfString != nullptr && std::strlen(_sdfString) > 0) - { - gzerr << "Both an SDF file and playback flag are specified. " - << "Only specify one.\n"; - return -1; - } - else - { - gzmsg << "Playing back states" << _playback << std::endl; - serverConfig.SetLogPlaybackPath(gz::common::absPath( - std::string(_playback))); - } - } - - // Create the Gazebo server - gz::sim::Server server(serverConfig); - - // Run the server - server.Run(true, _iterations, _run == 0); - - gzdbg << "Shutting down server" << std::endl; - return 0; -} - -////////////////////////////////////////////////// -extern "C" int runGui(const char *_guiConfig) -{ - gz::common::SignalHandler sigHandler; - bool sigKilled = false; - sigHandler.AddCallback([&](const int /*_sig*/) - { - sigKilled = true; - }); - - gzmsg << "Ignition Gazebo GUI v" << GZ_SIM_VERSION_FULL - << std::endl; - - int argc = 0; - char **argv = nullptr; - - // Initialize Qt app - gz::gui::Application app(argc, argv); - app.AddPluginPath(GZ_SIM_WORLD_INSTALL_DIR;; - - // add import path so we can load custom modules - app.Engine()->addImportPath(GZ_SIM_WORLD_INSTALL_DIR;; - - // Set default config file for Gazebo - std::string defaultConfigDir; - gz::common::env(IGN_HOMEDIR, defaultConfigDir); - defaultConfigDir = gz::common::joinPaths(defaultConfig, ".gz", - "sim", GZ_SIM_MAJOR_VERSION_STR); - - auto defaultConfig = gz::common::joinPaths(defaultConfigDir, - "gui.config"); - - app.SetDefaultConfigPath(defaultConfig); - - // Customize window - auto mainWin = app.findChild(); - auto win = mainWin->QuickWindow(); - win->setProperty("title", "Gazebo Sim"); - - // Instantiate GazeboDrawer.qml file into a component - QQmlComponent component(app.Engine(), ":/Gazebo/GazeboDrawer.qml"); - auto gzDrawerItem = qobject_cast(component.create(context)); - if (gzDrawerItem) - { - // C++ ownership - QQmlEngine::setObjectOwnership(gzDrawerItem, QQmlEngine::CppOwnership); - - // Add to main window - auto parentDrawerItem = win->findChild("sideDrawer"); - gzDrawerItem->setParentItem(parentDrawerItem); - gzDrawerItem->setParent(app.Engine()); - } - else - { - gzerr << "Failed to instantiate custom drawer, drawer will be empty" - << std::endl; - } - - // Get list of worlds - gz::transport::Node node; - - bool executed{false}; - bool result{false}; - unsigned int timeout{5000}; - std::string service{"/gazebo/worlds"}; - gz::msgs::StringMsg_V worldsMsg; - - // This loop is here to allow the server time to download resources. - // \todo(nkoenig) Async resource download. Search for "Async resource - // download in `src/Server.cc` for corresponding todo item. This todo is - // resolved when this while loop can be removed. - while (!sigKilled && !executed) - { - gzdbg << "GUI requesting list of world names. The server may be busy " - << "downloading resources. Please be patient." << std::endl; - executed = node.Request(service, timeout, worldsMsg, result); - } - - // Only print error message if a sigkill was not received. - if (!sigKilled) - { - if (!executed) - gzerr << "Timed out when getting world names." << std::endl; - else if (!result) - gzerr << "Failed to get world names." << std::endl; - } - - if (!executed || !result || worldsMsg.data().empty()) - return false; - - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - std::vector runners; -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - - // Configuration file from command line - if (_guiConfig != nullptr && std::strlen(_guiConfig) > 0) - { - if (!app.LoadConfig(_guiConfig)) - { - gzwarn << "Failed to load config file[" << _guiConfig << "]." - << std::endl; - } - - // Use the first world name with the config file - // TODO(anyone) Most of gz-sim's transport API includes the world name, - // which makes it complicated to mix configurations across worlds. - // We could have a way to use world-agnostic topics like Gazebo-classic's ~ - - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif - auto runner = new gz::sim::GuiRunner(worldsMsg.data(0)); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif - runner->connect(&app, &gz::gui::Application::PluginAdded, runner, - &gz::sim::GuiRunner::OnPluginAdded); - runners.push_back(runner); - runner->setParent(gz::gui::App()); - } - // GUI configuration from SDF (request to server) - else - { - // TODO(anyone) Parallelize this if multiple worlds becomes an important use - // case. - for (int w = 0; w < worldsMsg.data_size(); ++w) - { - const auto &worldName = worldsMsg.data(w); - - // Request GUI info for each world - result = false; - service = std::string("/world/" + worldName + "/gui/info"); - - gzdbg << "Requesting GUI from [" << service << "]..." << std::endl; - - // Request and block - gz::msgs::GUI res; - executed = node.Request(service, timeout, res, result); - - if (!executed) - gzerr << "Service call timed out for [" << service << "]" << std::endl; - else if (!result) - gzerr << "Service call failed for [" << service << "]" << std::endl; - - for (int p = 0; p < res.plugin_size(); ++p) - { - const auto &plugin = res.plugin(p); - const auto &fileName = plugin.filename(); - std::string pluginStr = "" + - plugin.innerxml() + ""; - - tinyxml2::XMLDocument pluginDoc; - pluginDoc.Parse(pluginStr.c_str()); - - app.LoadPlugin(fileName, - pluginDoc.FirstChildElement("plugin")); - } - - // GUI runner - auto runner = new gz::sim::GuiRunner(worldName); - runner->connect(&app, &gz::gui::Application::PluginAdded, runner, - &gz::sim::GuiRunner::OnPluginAdded); - runner->setParent(gz::gui::App()); - runners.push_back(runner); - } - mainWin->configChanged(); - } - - if (runners.empty()) - { - gzerr << "Failed to start a GUI runner." << std::endl; - return -1; - } - - // If no plugins have been added, load default config file - auto plugins = mainWin->findChildren(); - if (plugins.empty()) - { - // Check if there's a default config file under - // ~/.gz/sim and use that. If there isn't, copy - // the installed file there first. - if (!gz::common::exists(defaultConfig)) - { - auto installedConfig = gz::common::joinPaths( - GZ_SIM_GUI_CONFIG_PATH, "gui.config"); - - if (!gz::common::createDirectories(defaultConfigDir)) - { - gzerr << "Failed to create directory [" << defaultConfigDir - << "]." << std::endl; - return -1; - } - - if (!gz::common::exists(installedConfig)) - { - gzerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << "(file " << installedConfig << " doesn't exist)" - << std::endl; - return -1; - } - - if (!gz::common::copyFile(installedConfig, defaultConfig)) - { - gzerr << "Failed to copy installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - return -1; - } - else - { - gzmsg << "Copied installed config [" << installedConfig - << "] to default config [" << defaultConfig << "]." - << std::endl; - } - } - - // Also set ~/.gz/sim/ver/gui.config as the default path - if (!app.LoadConfig(defaultConfig)) - { - gzerr << "Failed to load config file[" << _guiConfig << "]." - << std::endl; - return -1; - } - } - - // Run main window. - // This blocks until the window is closed or we receive a SIGINT - app.exec(); - - for (auto runner : runners) - delete runner; - runners.clear(); - - gzdbg << "Shutting down GUI" << std::endl; - return 0; -} diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index d4bc795abc..53119092b1 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -2,7 +2,9 @@ gz_add_gui_plugin(ComponentInspector SOURCES ComponentInspector.cc Pose3d.cc + SystemPluginInfo.cc QT_HEADERS ComponentInspector.hh Pose3d.hh + SystemPluginInfo.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index ccea924b86..f025af4258 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -62,6 +62,7 @@ #include "gz/sim/components/SourceFilePath.hh" #include "gz/sim/components/SphericalCoordinates.hh" #include "gz/sim/components/Static.hh" +#include "gz/sim/components/SystemPluginInfo.hh" #include "gz/sim/components/ThreadPitch.hh" #include "gz/sim/components/Transparency.hh" #include "gz/sim/components/Visual.hh" @@ -73,6 +74,7 @@ #include "ComponentInspector.hh" #include "Pose3d.hh" +#include "SystemPluginInfo.hh" namespace gz::sim { @@ -114,6 +116,9 @@ namespace gz::sim /// \brief Handles all components displayed as a 3D pose. public: std::unique_ptr pose3d; + + /// \brief Handles all system info components. + public: std::unique_ptr systemInfo; }; } @@ -445,6 +450,8 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) // Type-specific handlers this->dataPtr->pose3d = std::make_unique(this); + this->dataPtr->systemInfo = + std::make_unique(this); } ////////////////////////////////////////////////// diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index 74f09f66a2..6dbccb673b 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -245,6 +245,14 @@ Rectangle { delegate: Loader { id: loader + + /** + * Most items can access model.data directly, but items like ListView, + * which have their own `model` property, can't. They can use + * `componentData` instead. + */ + property var componentData: model.data + source: delegateQml(model) } } diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 79b7d8a30d..24b9a85cfb 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -12,6 +12,7 @@ Pose3d.qml SphericalCoordinates.qml String.qml + SystemPluginInfo.qml TypeHeader.qml Vector3d.qml plottable_icon.svg diff --git a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml index c233ecf775..f36a0c6f6f 100644 --- a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml +++ b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml @@ -34,6 +34,9 @@ Rectangle { // Left indentation property int indentation: 10 + // Expose TypeHeader's custom tooltip + property alias customToolTip: typeHeader.customToolTip + RowLayout { anchors.fill: parent Item { @@ -55,7 +58,17 @@ Rectangle { Layout.fillWidth: true } } + ToolTip { + visible: ma.containsMouse + delay: tooltipDelay + text: typeHeader.customToolTip.length > 0 ? + typeHeader.customToolTip : typeHeader.tooltipText(componentData) + y: typeHeader.y - 30 + enter: null + exit: null + } MouseArea { + id: ma anchors.fill: parent hoverEnabled: true cursorShape: Qt.PointingHandCursor diff --git a/src/gui/plugins/component_inspector/SystemPluginInfo.cc b/src/gui/plugins/component_inspector/SystemPluginInfo.cc new file mode 100644 index 0000000000..7d64c35328 --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.cc @@ -0,0 +1,71 @@ +/* + * 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. + * +*/ +#include + +#include +#include + +#include "SystemPluginInfo.hh" +#include "ComponentInspector.hh" +#include "Types.hh" + +using namespace gz; +using namespace sim; +using namespace inspector; + +///////////////////////////////////////////////// +SystemPluginInfo::SystemPluginInfo(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("SystemPluginInfoImpl", this); + this->inspector = _inspector; + + this->inspector->AddUpdateViewCb(components::SystemPluginInfo::typeId, + std::bind(&SystemPluginInfo::UpdateView, this, std::placeholders::_1, + std::placeholders::_2)); +} + +///////////////////////////////////////////////// +void SystemPluginInfo::UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item) +{ + auto comp = _ecm.Component( + this->inspector->GetEntity()); + if (nullptr == _item || nullptr == comp) + return; + + auto msg = comp->Data(); + + _item->setData(QString("SystemPluginInfo"), + ComponentsModel::RoleNames().key("dataType")); + + QList pluginList; + for (int i = 0; i < msg.plugins().size(); ++i) + { + QList dataList; + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).name()))); + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).filename()))); + dataList.push_back( + QVariant(QString::fromStdString(msg.plugins(i).innerxml()))); + pluginList.push_back(dataList); + } + + _item->setData(pluginList, + ComponentsModel::RoleNames().key("data")); +} + diff --git a/src/gui/plugins/component_inspector/SystemPluginInfo.hh b/src/gui/plugins/component_inspector/SystemPluginInfo.hh new file mode 100644 index 0000000000..48cd39b037 --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.hh @@ -0,0 +1,54 @@ +/* + * 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 GZ_SIM_GUI_COMPONENTINSPECTOR_SYSTEMINFO_HH_ +#define GZ_SIM_GUI_COMPONENTINSPECTOR_SYSTEMINFO_HH_ + +#include "gz/sim/EntityComponentManager.hh" + +#include +#include + +namespace gz +{ +namespace sim +{ +class ComponentInspector; +namespace inspector +{ + /// \brief A class that handles SystemPluginInfo components. + class SystemPluginInfo : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit SystemPluginInfo(ComponentInspector *_inspector); + + /// \brief Callback when there are ECM updates. + /// \param[in] _ecm Immutable reference to the ECM. + /// \param[in] _item Item to update. + public: void UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item); + + /// \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/SystemPluginInfo.qml b/src/gui/plugins/component_inspector/SystemPluginInfo.qml new file mode 100644 index 0000000000..2e4d487b7f --- /dev/null +++ b/src/gui/plugins/component_inspector/SystemPluginInfo.qml @@ -0,0 +1,235 @@ +/* + * 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. + * +*/ +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" + +Rectangle { + id: systemInfoComponent + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + // Light text color according to theme + property string propertyColor: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + + // Dark text color according to theme + property string valueColor: Material.theme == Material.Light ? "black" : "white" + + Column { + anchors.fill: parent + + ExpandingTypeHeader { + id: header + customToolTip: "Information about system plugins attached to this entity" + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? column.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + ListView { + id: column + height: contentHeight + model: componentData + width: content.width + spacing: 5 + + delegate: Column { + width: content.width + height: pluginHeader.height + pluginContent.height + + Rectangle { + id: pluginHeader + width: content.width + color: "transparent" + height: 20 + RowLayout { + anchors.fill: parent + Item { + width: margin * 2 + indentation + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: pluginContent.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + Text { + height: parent.height + text: modelData[0].substring(modelData[0].lastIndexOf('::') + 2) + Layout.alignment : Qt.AlignVCenter + leftPadding: margin + color: propertyColor + font.pointSize: 12 + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: pluginHeader + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + pluginContent.show = !pluginContent.show + } + onEntered: { + pluginHeader.color = highlightColor + } + onExited: { + pluginHeader.color = "transparent" + } + } + } + GridLayout { + id: pluginContent + property bool show: false + width: parent.width + height: show ? 20 * 4 + innerXmlText.height : 0 + clip: true + columns: 4 + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + Item { + Layout.rowSpan: 4 + width: margin * 4 + indentation * 2 + } + Text { + height: 20 + text: "Name" + color: propertyColor + font.pointSize: 12 + } + Text { + height: 20 + text: modelData[0] + color: valueColor + font.pointSize: 12 + clip: true + elide: Text.ElideLeft + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + + ToolTip { + visible: maName.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: modelData[0] + enter: null + exit: null + } + MouseArea { + id: maName + anchors.fill: parent + hoverEnabled: true + } + } + Item { + Layout.rowSpan: 4 + width: margin + } + Text { + height: 20 + text: "Filename" + color: propertyColor + font.pointSize: 12 + } + Text { + height: 20 + text: modelData[1] + color: valueColor + font.pointSize: 12 + clip: true + elide: Text.ElideLeft + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + + ToolTip { + visible: maFilename.containsMouse + delay: Qt.styleHints.mousePressAndHoldInterval + text: modelData[1] + enter: null + exit: null + } + MouseArea { + id: maFilename + anchors.fill: parent + hoverEnabled: true + } + } + Text { + height: 20 + text: "Inner XML" + color: propertyColor + font.pointSize: 12 + Layout.columnSpan: 2 + } + Rectangle { + id: innerXmlRectangle + Layout.columnSpan: 2 + Layout.fillWidth: true + height: innerXmlText.paintedHeight + color: Material.background + TextEdit { + id: innerXmlText + width: parent.width + text: modelData[2].length == 0 ? "N/A" : modelData[2].trim() + color: valueColor + textFormat: TextEdit.PlainText + font.pointSize: 10 + clip: true + readOnly: true + wrapMode: Text.WordWrap + selectByMouse: true + } + } + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/TypeHeader.qml b/src/gui/plugins/component_inspector/TypeHeader.qml index 5cb0b39e2b..d8dd9221d5 100644 --- a/src/gui/plugins/component_inspector/TypeHeader.qml +++ b/src/gui/plugins/component_inspector/TypeHeader.qml @@ -27,6 +27,10 @@ Rectangle { width: headerText.width color: "transparent" + // Custom tooltip text. If not set, the component's type name and ID will be + // shown. + property string customToolTip: "" + function tooltipText(_model) { if (model === null) return "Unknown component" @@ -50,7 +54,7 @@ Rectangle { ToolTip { visible: ma.containsMouse delay: tooltipDelay - text: tooltipText(model) + text: customToolTip.length > 0 ? customToolTip : typeHeader.tooltipText(componentData) y: typeHeader.y - 30 enter: null exit: null diff --git a/src/gui/plugins/component_inspector_editor/Pose3d.qml b/src/gui/plugins/component_inspector_editor/Pose3d.qml index 8b2acf20ac..40e3bf9ba9 100644 --- a/src/gui/plugins/component_inspector_editor/Pose3d.qml +++ b/src/gui/plugins/component_inspector_editor/Pose3d.qml @@ -315,7 +315,7 @@ Rectangle { id: zSpin Layout.fillWidth: true height: 40 - numberValue: model.data[2] + numberValue: model.data[2] minValue: -Number.MAX_VALUE maxValue: Number.MAX_VALUE stepValue: 0.1 diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index a6755bfcb4..0b532d58d5 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,7 @@ #include #include #include +#include #include #include "gz/sim/components/CastShadows.hh" @@ -1318,6 +1320,26 @@ rendering::GeometryPtr VisualizationCapabilitiesPrivate::CreateGeometry( } scale = _geom.HeightmapShape()->Size(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->scene->CreateMesh(descriptor); + } else { gzerr << "Unsupported geometry type" << std::endl; diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 8ff5d5a02c..0c0ba40fbd 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -1032,6 +1032,8 @@ void RenderUtil::Update() return; this->dataPtr->updateMutex.lock(); + + this->dataPtr->scene->SetTime(this->dataPtr->simTime); auto newScenes = std::move(this->dataPtr->newScenes); auto newModels = std::move(this->dataPtr->newModels); auto newLinks = std::move(this->dataPtr->newLinks); @@ -2543,6 +2545,7 @@ void RenderUtil::Init() this->dataPtr->scene->SetSkyEnabled(this->dataPtr->skyEnabled); } } + this->dataPtr->sceneManager.SetScene(this->dataPtr->scene); if (this->dataPtr->enableSensors) this->dataPtr->markerManager.SetTopic("/sensors/marker"); diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 3efcb359c5..7ca02308d5 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include @@ -41,6 +42,7 @@ #include #include #include +#include #include @@ -757,6 +759,26 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, } scale = _geom.HeightmapShape()->Size(); } + else if (_geom.Type() == sdf::GeometryType::POLYLINE) + { + std::vector> vertices; + for (const auto &polyline : _geom.PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + + auto meshManager = common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom.PolylineShape()[0].Height()); + + rendering::MeshDescriptor descriptor; + descriptor.meshName = name; + descriptor.mesh = meshManager->MeshByName(name); + + geom = this->dataPtr->scene->CreateMesh(descriptor); + } else { gzerr << "Unsupported geometry type" << std::endl; diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index 8f03d849e8..b8a8c312c0 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -140,6 +140,24 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AirPressurePrivate::UpdatePressures function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAirPressures(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index f70bce2deb..686de58feb 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -141,6 +141,24 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AltimeterPrivate::UpdateAltimeters function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateAltimeters(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index c4f606237c..a99ea0872a 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -132,6 +132,24 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ForceTorquePrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 3c8e115388..f026226917 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -251,7 +251,7 @@ void Hydrodynamics::Configure( if (!_sdf->HasElement("link_name")) { - gzerr << "You musk specify a for the hydrodynamic" + gzerr << "You must specify a for the hydrodynamic" << " plugin to act upon"; return; } diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index 71429e922e..98e32b89d4 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -41,27 +41,27 @@ namespace systems /// plugin), etc. /// /// # System Parameters - /// The exact description of these parameters can be found on p. 37 of - /// Fossen's book. They are used to calculate added mass, linear and quadratic - /// drag and coriolis force. + /// The exact description of these parameters can be found on p. 37 and + /// p. 43 of Fossen's book. They are used to calculate added mass, linear and + /// quadratic drag and coriolis force. /// * - Added mass in x direction [kg] /// * - Added mass in y direction [kg] /// * - Added mass in z direction [kg] /// * - Added mass in roll direction [kgm^2] /// * - Added mass in pitch direction [kgm^2] /// * - Added mass in yaw direction [kgm^2] - /// * - Stability derivative, 2nd order, x component [kg/m] - /// * - Stability derivative, 1st order, x component [kg] - /// * - Stability derivative, 2nd order, y component [kg/m] - /// * - Stability derivative, 1st order, y component [kg] - /// * - Stability derivative, 2nd order, z component [kg/m] - /// * - Stability derivative, 1st order, z component [kg] - /// * - Stability derivative, 2nd order, roll component [kg/m^2] - /// * - Stability derivative, 1st order, roll component [kg/m] - /// * - Stability derivative, 2nd order, pitch component [kg/m^2] - /// * - Stability derivative, 1st order, pitch component [kg/m] - /// * - Stability derivative, 2nd order, yaw component [kg/m^2] - /// * - Stability derivative, 1st order, yaw component [kg/m] + /// * - Quadratic damping, 2nd order, x component [kg/m] + /// * - Linear damping, 1st order, x component [kg] + /// * - Quadratic damping, 2nd order, y component [kg/m] + /// * - Linear damping, 1st order, y component [kg] + /// * - Quadratic damping, 2nd order, z component [kg/m] + /// * - Linear damping, 1st order, z component [kg] + /// * - Quadratic damping, 2nd order, roll component [kg/m^2] + /// * - Linear damping, 1st order, roll component [kg/m] + /// * - Quadratic damping, 2nd order, pitch component [kg/m^2] + /// * - Linear damping, 1st order, pitch component [kg/m] + /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] + /// * - Linear damping, 1st order, yaw component [kg/m] /// Additionally the system also supports the following parameters: /// * - The density of the fluid its moving in. /// Defaults to 998kgm^-3. [kgm^-3, deprecated] diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 00a658e349..fb79253eaf 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -145,6 +145,24 @@ void Imu::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the ImuPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 57b813ae05..7714a5dbc9 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -143,6 +143,25 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the LogicalCameraPrivate::UpdateLogicalCameras + // function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->UpdateLogicalCameras(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 9d54231155..032afada3e 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -142,6 +142,24 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the MagnetometerPrivate::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.cc b/src/systems/multicopter_control/MulticopterVelocityControl.cc index f050fbde7e..d9f77f56e1 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.cc +++ b/src/systems/multicopter_control/MulticopterVelocityControl.cc @@ -36,6 +36,7 @@ #include "gz/sim/components/Gravity.hh" #include "gz/sim/components/Inertial.hh" #include "gz/sim/components/Link.hh" +#include "gz/sim/components/Model.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Link.hh" @@ -92,20 +93,7 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, VehicleParameters vehicleParams; math::Inertiald vehicleInertial; - // Compute the vehicle's moment of inertia and mass assuming that all the - // links in the model belong to the vehicle. - for (const Entity &link : - _ecm.ChildrenByComponents(this->model.Entity(), components::Link())) - { - auto inertial = _ecm.Component(link); - if (nullptr == inertial) - { - gzerr << "Could not find inertial component on on link " - << this->comLinkName << std::endl; - return; - } - vehicleInertial += inertial->Data(); - } + vehicleInertial = this->VehicleInertial(_ecm, this->model.Entity()); vehicleParams.mass = vehicleInertial.MassMatrix().Mass(); vehicleParams.inertia = math::eigen3::convert(vehicleInertial.Moi()); @@ -318,6 +306,33 @@ void MulticopterVelocityControl::Configure(const Entity &_entity, this->initialized = true; } +////////////////////////////////////////////////// +math::Inertiald MulticopterVelocityControl::VehicleInertial( + const EntityComponentManager &_ecm, Entity _entity) +{ + math::Inertiald vehicleInertial; + + for (const Entity &link : + _ecm.ChildrenByComponents(_entity, components::Link())) + { + auto inertial = _ecm.Component(link); + if (nullptr == inertial) + { + gzerr << "Could not find inertial component on link " + << this->comLinkName << std::endl; + return vehicleInertial; + } + vehicleInertial += inertial->Data(); + } + + for (const Entity &modelEnt : + _ecm.ChildrenByComponents(_entity, components::Model())) + { + vehicleInertial += this->VehicleInertial(_ecm, modelEnt); + } + return vehicleInertial; +} + ////////////////////////////////////////////////// void MulticopterVelocityControl::PreUpdate( const gz::sim::UpdateInfo &_info, diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.hh b/src/systems/multicopter_control/MulticopterVelocityControl.hh index 8f035e3cd3..67e1398011 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.hh +++ b/src/systems/multicopter_control/MulticopterVelocityControl.hh @@ -185,6 +185,13 @@ namespace systems gz::sim::EntityComponentManager &_ecm, const Eigen::VectorXd &_vels); + /// \brief Get the vehicle inertial from child links and nested models + /// \param[in] _ecm Immutable reference to the EntityComponentManager + /// \param[in] _entity Model entity to get inertial for + private: math::Inertiald VehicleInertial( + const EntityComponentManager &_ecm, + Entity _entity); + /// \brief Model interface private: Model model{kNullEntity}; diff --git a/src/systems/navsat/NavSat.cc b/src/systems/navsat/NavSat.cc index ead58a5ce1..1b52d0cc60 100644 --- a/src/systems/navsat/NavSat.cc +++ b/src/systems/navsat/NavSat.cc @@ -137,6 +137,24 @@ void NavSat::PostUpdate(const UpdateInfo &_info, // Only update and publish if not paused. if (!_info.paused) { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: ign-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the NavSat::Implementation::Update function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + this->dataPtr->Update(_ecm); for (auto &it : this->dataPtr->entitySensorMap) diff --git a/src/systems/physics/EntityFeatureMap.hh b/src/systems/physics/EntityFeatureMap.hh index 1946a0c78f..969d800a1a 100644 --- a/src/systems/physics/EntityFeatureMap.hh +++ b/src/systems/physics/EntityFeatureMap.hh @@ -203,6 +203,20 @@ namespace systems::physics_system return nullptr; } + /// \brief Get Gazebo entity that corresponds to the physics entity ID + /// \param[in] _id Physics entity ID + /// \return If found, returns the corresponding Gazebo entity. Otherwise, + /// kNullEntity + public: Entity GetByPhysicsId(std::size_t _id) const + { + auto it = this->entityByPhysId.find(_id); + if (it != this->entityByPhysId.end()) + { + return it->second; + } + return kNullEntity; + } + /// \brief Check whether there is a physics entity associated with the given /// Gazebo entity /// \param[in] _entity Gazebo entity. @@ -232,6 +246,7 @@ namespace systems::physics_system this->entityMap[_entity] = _physicsEntity; this->reverseMap[_physicsEntity] = _entity; this->physEntityById[_physicsEntity->EntityID()] = _physicsEntity; + this->entityByPhysId[_physicsEntity->EntityID()] = _entity; } /// \brief Remove entity from all associated maps @@ -244,6 +259,7 @@ namespace systems::physics_system { this->reverseMap.erase(it->second); this->physEntityById.erase(it->second->EntityID()); + this->entityByPhysId.erase(it->second->EntityID()); this->castCache.erase(_entity); this->entityMap.erase(it); return true; @@ -261,6 +277,7 @@ namespace systems::physics_system { this->entityMap.erase(it->second); this->physEntityById.erase(it->first->EntityID()); + this->entityByPhysId.erase(it->first->EntityID()); this->castCache.erase(it->second); this->reverseMap.erase(it); return true; @@ -282,7 +299,8 @@ namespace systems::physics_system public: std::size_t TotalMapEntryCount() const { return this->entityMap.size() + this->reverseMap.size() + - this->castCache.size() + this->physEntityById.size(); + this->castCache.size() + this->physEntityById.size() + + this->entityByPhysId.size(); } /// \brief Map from Gazebo entity to physics entities with required features @@ -295,6 +313,9 @@ namespace systems::physics_system /// with required features private: std::unordered_map physEntityById; + /// \brief Map of physics entity IDs to Gazebo entity + private: std::unordered_map entityByPhysId; + /// \brief Cache map from Gazebo entity to physics entities with optional /// features private: mutable std::unordered_map castCache; diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index 155aa884fe..b3e5f9223d 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -124,17 +124,18 @@ TEST_F(EntityFeatureMapFixture, testMap.AddEntity(gazeboWorld1Entity, testWorld1); - // After adding the entity, there should be one entry each in three maps - EXPECT_EQ(3u, testMap.TotalMapEntryCount()); + // After adding the entity, there should be one entry each in four maps + EXPECT_EQ(4u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld1, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(gazeboWorld1Entity, testMap.Get(testWorld1)); + EXPECT_EQ(gazeboWorld1Entity, testMap.GetByPhysicsId(testWorld1->EntityID())); // Cast to optional feature1 auto testWorld1Feature1 = testMap.EntityCast(gazeboWorld1Entity); ASSERT_NE(nullptr, testWorld1Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); // Cast to optional feature2 auto testWorld1Feature2 = @@ -142,38 +143,43 @@ TEST_F(EntityFeatureMapFixture, ASSERT_NE(nullptr, testWorld1Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); // Add another entity WorldPtrType testWorld2 = this->engine->ConstructEmptyWorld("world2"); testMap.AddEntity(gazeboWorld2Entity, testWorld2); - EXPECT_EQ(7u, testMap.TotalMapEntryCount()); + EXPECT_EQ(9u, testMap.TotalMapEntryCount()); EXPECT_EQ(testWorld2, testMap.Get(gazeboWorld2Entity)); EXPECT_EQ(gazeboWorld2Entity, testMap.Get(testWorld2)); + EXPECT_EQ(gazeboWorld2Entity, testMap.GetByPhysicsId(testWorld2->EntityID())); auto testWorld2Feature1 = testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature1); // After the cast, there should be one more entry in the cache map. - EXPECT_EQ(8u, testMap.TotalMapEntryCount()); + EXPECT_EQ(10u, testMap.TotalMapEntryCount()); auto testWorld2Feature2 = testMap.EntityCast(testWorld2); ASSERT_NE(nullptr, testWorld2Feature2); // After the cast, the number of entries should remain the same because we // have not added an entity. - EXPECT_EQ(8u, testMap.TotalMapEntryCount()); + EXPECT_EQ(10u, testMap.TotalMapEntryCount()); // Remove entitites testMap.Remove(gazeboWorld1Entity); EXPECT_FALSE(testMap.HasEntity(gazeboWorld1Entity)); EXPECT_EQ(nullptr, testMap.Get(gazeboWorld1Entity)); EXPECT_EQ(sim::kNullEntity, testMap.Get(testWorld1)); - EXPECT_EQ(4u, testMap.TotalMapEntryCount()); + EXPECT_EQ(sim::kNullEntity, + testMap.GetByPhysicsId(testWorld1->EntityID())); + EXPECT_EQ(5u, testMap.TotalMapEntryCount()); testMap.Remove(testWorld2); EXPECT_FALSE(testMap.HasEntity(gazeboWorld2Entity)); EXPECT_EQ(nullptr, testMap.Get(gazeboWorld2Entity)); EXPECT_EQ(sim::kNullEntity, testMap.Get(testWorld2)); + EXPECT_EQ(sim::kNullEntity, testMap.GetByPhysicsId( + testWorld2->EntityID())); EXPECT_EQ(0u, testMap.TotalMapEntryCount()); } diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 33e913a3b3..e2e31063c0 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -82,6 +83,7 @@ #include #include #include +#include #include #include @@ -1370,6 +1372,55 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, math::eigen3::convert(heightmapSdf->Size()), heightmapSdf->Sampling()); } + else if (_geom->Data().Type() == sdf::GeometryType::POLYLINE) + { + auto polylineSdf = _geom->Data().PolylineShape(); + if (polylineSdf.empty()) + { + gzwarn << "Polyline geometry for collision [" << _name->Data() + << "] missing polylines." << std::endl; + return true; + } + + std::vector> vertices; + for (const auto &polyline : _geom->Data().PolylineShape()) + { + vertices.push_back(polyline.Points()); + } + + std::string name("POLYLINE_" + common::Uuid().String()); + auto meshManager = common::MeshManager::Instance(); + meshManager->CreateExtrudedPolyline(name, vertices, + _geom->Data().PolylineShape()[0].Height()); + + auto polyline = meshManager->MeshByName(name); + if (nullptr == polyline) + { + gzwarn << "Failed to create polyline for collision [" + << _name->Data() << "]." << std::endl; + return true; + } + + auto linkMeshFeature = + this->entityLinkMap.EntityCast(_parent->Data()); + if (!linkMeshFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to process polyline geometries, but the" + << " physics engine doesn't support feature " + << "[AttachMeshShapeFeature]. Polylines will be ignored." + << std::endl; + informed = true; + } + return true; + } + + collisionPtrPhys = linkMeshFeature->AttachMeshShape(_name->Data(), + *polyline, + math::eigen3::convert(_pose->Data())); + } else { auto linkCollisionFeature = @@ -3425,15 +3476,16 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) // Note that we are temporarily storing pointers to elements in this // ("allContacts") container. Thus, we must make sure it doesn't get destroyed // until the end of this function. - auto allContacts = worldCollisionFeature->GetContactsFromLastStep(); + auto allContacts = + std::move(worldCollisionFeature->GetContactsFromLastStep()); + for (const auto &contactComposite : allContacts) { const auto &contact = contactComposite.Get(); auto coll1Entity = - this->entityCollisionMap.Get(ShapePtrType(contact.collision1)); + this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); auto coll2Entity = - this->entityCollisionMap.Get(ShapePtrType(contact.collision2)); - + this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { @@ -3532,10 +3584,10 @@ void PhysicsPrivate::EnableContactSurfaceCustomization(const Entity &_world) Feature::ContactSurfaceParams &_params) { const auto &contact = _contact.Get(); - auto coll1Entity = this->entityCollisionMap.Get( - ShapePtrType(contact.collision1)); - auto coll2Entity = this->entityCollisionMap.Get( - ShapePtrType(contact.collision2)); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contact.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contact.collision2->EntityID()); // check if at least one of the entities wants contact surface // customization diff --git a/src/systems/rf_comms/RFComms.cc b/src/systems/rf_comms/RFComms.cc index 40930c5d05..ce2a2841f2 100644 --- a/src/systems/rf_comms/RFComms.cc +++ b/src/systems/rf_comms/RFComms.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -447,7 +449,18 @@ void RFComms::Step( #endif if (sendPacket) - _newRegistry[msg->dst_address()].inboundMsgs.push_back(msg); + { + // We create a copy of the outbound message because each destination + // might have a different rssi value. + auto inboundMsg = std::make_shared(*msg); + + // Add rssi. + auto *rssiPtr = inboundMsg->mutable_header()->add_data(); + rssiPtr->set_key("rssi"); + rssiPtr->add_value(std::to_string(rssi)); + + _newRegistry[msg->dst_address()].inboundMsgs.push_back(inboundMsg); + } } } diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index e00c7bc8dd..f1ac9a492e 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -51,6 +51,7 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/RgbdCamera.hh" +#include "gz/sim/components/Scene.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Static.hh" #include "gz/sim/components/ThermalCamera.hh" @@ -63,6 +64,7 @@ #include #include #include +#include #include using namespace std::chrono_literals; @@ -224,15 +226,22 @@ class gz::sim::systems::SceneBroadcasterPrivate /// paused. The not-paused (a.ka. running) period has a key=false and a /// default update rate of 60Hz. The paused period has a key=true and a /// default update rate of 30Hz. - public: std::map>> - statePublishPeriod{{false, std::chrono::milliseconds(1000/60)}, - {true, std::chrono::milliseconds(1000/30)}}; + public: std::map>> + statePublishPeriod{ + {false, std::chrono::duration>(1000/60.0)}, + {true, std::chrono::duration>(1000/30.0)}}; /// \brief Flag used to indicate if the state service was called. public: bool stateServiceRequest{false}; /// \brief A list of async state requests public: std::unordered_set stateRequests; + + /// \brief Store SDF scene information so that it can be inserted into + /// scene message. + public: sdf::Scene sdfScene; }; ////////////////////////////////////////////////// @@ -261,16 +270,25 @@ void SceneBroadcaster::Configure( auto readHertz = _sdf->Get("dynamic_pose_hertz", 60); this->dataPtr->dyPoseHertz = readHertz.first; - auto stateHerz = _sdf->Get("state_hertz", 60); - this->dataPtr->statePublishPeriod[false] = - std::chrono::duration>( - std::chrono::milliseconds(1000/stateHerz.first)); - - // Set the paused update rate to half of the running update rate. - this->dataPtr->statePublishPeriod[true] = - std::chrono::duration>( - std::chrono::milliseconds(1000 / - static_cast(std::max(stateHerz.first * 0.5, 1.0)))); + auto stateHertz = _sdf->Get("state_hertz", 60); + if (stateHertz.first > 0.0) + { + this->dataPtr->statePublishPeriod[false] = + std::chrono::duration>( + 1000 / stateHertz.first); + + // Set the paused update rate to half of the running update rate. + this->dataPtr->statePublishPeriod[true] = + std::chrono::duration>(1000/ + (stateHertz.first * 0.5)); + } + else + { + using secs_double = std::chrono::duration>; + gzerr << "SceneBroadcaster state_hertz must be positive, using default (" + << 1.0 / secs_double(this->dataPtr->statePublishPeriod[false]).count() << + "Hz)\n"; + } // Add to graph { @@ -290,8 +308,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, if (_manager.HasNewEntities()) this->dataPtr->SceneGraphAddEntities(_manager); - // Populate pose message - // TODO(louise) Get from SDF + // Store the Scene component data, which holds sdf::Scene so that we can + // populate the scene info messages. + auto sceneComp = + _manager.Component(this->dataPtr->worldEntity); + if (sceneComp) + this->dataPtr->sdfScene = sceneComp->Data(); // Create and send pose update if transport connections exist. if (this->dataPtr->dyPosePub.HasConnections() || @@ -603,6 +625,7 @@ bool SceneBroadcasterPrivate::SceneInfoService(gz::msgs::Scene &_res) _res.Clear(); // Populate scene message + _res.CopyFrom(convert(this->sdfScene)); // Add models AddModels(&_res, this->worldEntity, this->sceneGraph); @@ -1000,6 +1023,8 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( this->SetupTransport(this->worldName); msgs::Scene sceneMsg; + // Populate scene message + sceneMsg.CopyFrom(convert(this->sdfScene)); AddModels(&sceneMsg, this->worldEntity, newGraph); diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 1e91e52ad5..97579c34ed 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -188,6 +189,11 @@ class gz::sim::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Check if sensor has subscribers + /// \param[in] _sensor Sensor to check + /// \return True if the sensor has subscribers, false otherwise + public: bool HasConnections(sensors::RenderingSensor *_sensor) const; + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -310,6 +316,7 @@ void SensorsPrivate::RunOnce() { IGN_PROFILE("PreRender"); this->eventManager->Emit(); + this->scene->SetTime(this->updateTime); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -317,12 +324,34 @@ void SensorsPrivate::RunOnce() this->scene->PreRender(); } + // disable sensors that have no subscribers to prevent doing unnecessary + // work + std::unordered_set tmpDisabledSensors; + this->sensorMaskMutex.lock(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + auto rs = dynamic_cast(s); + if (rs->IsActive() && !this->HasConnections(rs)) + { + rs->SetActive(false); + tmpDisabledSensors.insert(rs); + } + } + this->sensorMaskMutex.unlock(); + { // publish data IGN_PROFILE("RunOnce"); this->sensorManager.RunOnce(this->updateTime); } + // re-enble sensors + for (auto &rs : tmpDisabledSensors) + { + rs->SetActive(true); + } + { IGN_PROFILE("PostRender"); // Update the scene graph manually to improve performance @@ -812,6 +841,44 @@ std::string Sensors::CreateSensor(const Entity &_entity, return sensor->Name(); } +////////////////////////////////////////////////// +bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const +{ + if (!_sensor) + return true; + + // \todo(iche033) Remove this function once a virtual + // sensors::RenderingSensor::HasConnections function is available + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + { + auto s = dynamic_cast(_sensor); + if (s) + return s->HasConnections(); + } + gzwarn << "Unable to check connection count for sensor: " << _sensor->Name() + << ". Unknown sensor type." << std::endl; + return true; +} + IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemUpdate, diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 0042ea2e84..2b9cbe2199 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -27,6 +27,7 @@ #include #include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/BatterySoC.hh" #include "gz/sim/components/ChildLinkName.hh" #include "gz/sim/components/JointAxis.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -64,6 +65,12 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Desired propeller angular velocity in rad / s public: double propellerAngVel = 0.0; + /// \brief Enabled or not + public: bool enabled = true; + + /// \brief Model entity + public: Entity modelEntity; + /// \brief The link entity which will spin public: Entity linkEntity; @@ -110,21 +117,26 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Diameter of propeller in m, default: 0.02 public: double propellerDiameter = 0.02; - /// \brief callback for handling thrust update - public: void OnCmdThrust(const gz::msgs::Double &_msg); + /// \brief Callback for handling thrust update + public: void OnCmdThrust(const msgs::Double &_msg); /// \brief callback for handling angular velocity update public: void OnCmdAngVel(const gz::msgs::Double &_msg); - /// \brief function which computes angular velocity from thrust + /// \brief Function which computes angular velocity from thrust /// \param[in] _thrust Thrust in N /// \return Angular velocity in rad/s public: double ThrustToAngularVec(double _thrust); - /// \brief function which computers thrust from angular velocity + /// \brief Function which computers thrust from angular velocity /// \param[in] _angVel Angular Velocity in rad/s /// \return Thrust in Newtons public: double AngularVelToThrust(double _angVel); + + /// \brief Returns a boolean if the battery has sufficient charge to continue + /// \return True if battery is charged, false otherwise. If no battery found, + /// returns true. + public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; }; ///////////////////////////////////////////////// @@ -142,6 +154,7 @@ void Thruster::Configure( gz::sim::EventManager &/*_eventMgr*/) { // Create model object, to access convenient functions + this->dataPtr->modelEntity = _entity; auto model = gz::sim::Model(_entity); auto modelName = model.Name(_ecm); @@ -385,6 +398,28 @@ double ThrusterPrivateData::AngularVelToThrust(double _angVel) * abs(_angVel) * _angVel * this->fluidDensity; } +///////////////////////////////////////////////// +bool ThrusterPrivateData::HasSufficientBattery( + const EntityComponentManager &_ecm) const +{ + bool result = true; + _ecm.Each([&]( + const Entity &_entity, + const components::BatterySoC *_data + ){ + if(_ecm.ParentEntity(_entity) == this->modelEntity) + { + if(_data->Data() <= 0) + { + result = false; + } + } + + return true; + }); + return result; +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const gz::sim::UpdateInfo &_info, @@ -393,6 +428,11 @@ void Thruster::PreUpdate( if (_info.paused) return; + if (!this->dataPtr->enabled) + { + return; + } + gz::sim::Link link(this->dataPtr->linkEntity); // TODO(arjo129): add logic for custom coordinate frame @@ -460,10 +500,18 @@ void Thruster::PreUpdate( unitVector * torque); } +///////////////////////////////////////////////// +void Thruster::PostUpdate(const UpdateInfo &/*unused*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->enabled = this->dataPtr->HasSufficientBattery(_ecm); +} + IGNITION_ADD_PLUGIN( Thruster, System, Thruster::ISystemConfigure, - Thruster::ISystemPreUpdate) + Thruster::ISystemPreUpdate, + Thruster::ISystemPostUpdate) IGNITION_ADD_PLUGIN_ALIAS(Thruster, "gz::sim::systems::Thruster") diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index a85f00c249..2ca04b5156 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -106,7 +106,8 @@ namespace systems class Thruster: public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { /// \brief Constructor public: Thruster(); @@ -123,6 +124,10 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 339bee21be..a429e36cdf 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -88,8 +88,8 @@ set(tests_needing_display sensors_system.cc sensors_system_battery.cc shader_param_system.cc - thermal_system.cc thermal_sensor_system.cc + thermal_system.cc wide_angle_camera.cc ) @@ -129,7 +129,12 @@ ign_build_tests(TYPE INTEGRATION # For INTEGRATION_physics_system, we need to check what version of DART is # available so that we can disable tests that are unsupported by the particular # version of physics engine -ign_find_package(DART QUIET) +cmake_policy(PUSH) +if (POLICY CMP0074) + cmake_policy(SET CMP0074 NEW) +endif() +ign_find_package(DART CONFIG) +cmake_policy(POP) if (DART_FOUND) # Only adding include directories, no need to link against DART to check version target_include_directories(INTEGRATION_physics_system SYSTEM PRIVATE ${DART_INCLUDE_DIRS}) diff --git a/test/integration/multicopter.cc b/test/integration/multicopter.cc index afeddb2d6f..23c020990a 100644 --- a/test/integration/multicopter.cc +++ b/test/integration/multicopter.cc @@ -34,6 +34,7 @@ #include "gz/sim/components/Link.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Model.hh" +#include "gz/sim/components/Pose.hh" #include "gz/sim/Model.hh" #include "gz/sim/Server.hh" @@ -317,3 +318,65 @@ TEST_F(MulticopterTest, }); server->Run(true, 10, false); } + +///////////////////////////////////////////////// +TEST_F(MulticopterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(MulticopterVelocityControlNestedModel)) +{ + // test that the drone is able to take off when carrying a payload + // (nexted model) with extra mass. + + // Start server + auto server = + this->StartServer("/test/worlds/quadcopter_velocity_control_nested.sdf"); + + test::Relay testSystem; + transport::Node node; + auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); + + // Add the system + server->AddSystem(testSystem.systemPtr); + server->Run(true, 1, false); + + // get pose of drone in post update + math::Pose3d x3Pose; + testSystem.OnPostUpdate( + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + auto x3Ent = _ecm.EntityByComponents( + components::Model(), components::Name("X3")); + ASSERT_NE(kNullEntity, x3Ent); + + auto poseComp = _ecm.Component(x3Ent); + if (poseComp) + x3Pose = poseComp->Data(); + }); + + server->Run(true, 100, false); + + // check initial z pos + double initialZ = x3Pose.Pos().Z(); + EXPECT_GT(0.1, initialZ); + + // run for a few interations and verify drone is still on the ground + server->Run(true, 100, false); + EXPECT_NEAR(initialZ, x3Pose.Pos().Z(), 1e-3); + + // send linear z vel for drone to take off + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0, 0, 5)); + cmdVel.Publish(msg); + + // verify drone continues to fly higher over the duration of 1 second + double zHeight = x3Pose.Pos().Z(); + for (unsigned int i = 0; i < 10; ++i) + { + server->Run(true, 100, false); + EXPECT_LT(zHeight, x3Pose.Pos().Z()); + zHeight = x3Pose.Pos().Z(); + } + + // one last check to verify drone is at least 5 meters off the ground + EXPECT_LT(5.0, x3Pose.Pos().Z()); +} diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 8fc6cf01b5..73c06be711 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -2196,3 +2196,71 @@ TEST_F(PhysicsSystemFixture, EXPECT_NEAR(0.0, wrench.torque().z(), 1e-3); } } + +///////////////////////////////////////////////// +// Test that joint velocity limit is applied +TEST_F(PhysicsSystemFixtureWithDart6_10, + IGN_UTILS_TEST_DISABLED_ON_WIN32(JointVelocityLimitTest)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_velocity_limit.sdf"; + serverConfig.SetSdfFile(sdfFile); + + auto server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + EXPECT_FALSE(*server->Running(0)); + + server->SetUpdatePeriod(1ns); + + test::Relay testSystem; + + const std::size_t nIters{600}; + testSystem.OnPreUpdate( + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) + { + // Create components, if they don't exist on the first iteration + if (_info.iterations == 1) + { + for (const auto &e : _ecm.EntitiesByComponents(components::Joint())) + { + if (!_ecm.Component(e)) + { + _ecm.CreateComponent(e, components::JointVelocity()); + } + } + } + }); + + testSystem.OnPostUpdate( + [&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + // At nIters iterations, check angular velocity of each of the joints + if (_info.iterations == nIters) + { + int count = 0; + for (const auto &e : _ecm.EntitiesByComponents(components::Joint())) + { + auto *jointVel = _ecm.Component(e); + EXPECT_NE(nullptr, jointVel); + EXPECT_FALSE(jointVel->Data().empty()); + if (jointVel->Data().size() > 0) + { + ++count; + // Joint velocity should lie between + // - (kVelocityLimit + kTolerance) and + // + (kVelocityLimit + kTolerance) + const double kVelocityLimit = 1.0; + const double kTolerance = 1e-6; + EXPECT_NEAR(jointVel->Data()[0], 0, kVelocityLimit + kTolerance); + } + } + EXPECT_EQ(count, 2); + } + }); + + server->AddSystem(testSystem.systemPtr); + server->Run(true, nIters, false); +} diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc index 72c064674f..d25cd2e9e2 100644 --- a/test/integration/rf_comms.cc +++ b/test/integration/rf_comms.cc @@ -65,7 +65,10 @@ TEST_F(RFCommsTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) gz::msgs::StringMsg receivedMsg; receivedMsg.ParseFromString(_msg.data()); + EXPECT_EQ(expected, receivedMsg.data()); + ASSERT_GT(_msg.header().data_size(), 0); + EXPECT_EQ("rssi", _msg.header().data(0).key()); msgCounter++; }; diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 09804c46db..809a5119a9 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -874,6 +874,84 @@ TEST_P(SceneBroadcasterTest, EXPECT_GE(receivedStates, 7); } +///////////////////////////////////////////////// +// Tests https://github.com/ignitionrobotics/ign-gazebo/issues/1414 +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(DecimalStateHertz)) +{ + // Start server + std::string sdfStr = R"( + + + + + 0.001 + 1.0 + + + + + 0.4 + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + +)"; + sim::ServerConfig serverConfig; + serverConfig.SetSdfString(sdfStr); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); +} + +///////////////////////////////////////////////// +TEST_P(SceneBroadcasterTest, + IGN_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasSceneSdf)) +{ + // Start server + sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + common::joinPaths("/", "test", "worlds", "conveyor.sdf")); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/default/scene/info", timeout, res, result)); + EXPECT_TRUE(result); + + ASSERT_TRUE(res.has_ambient()); + EXPECT_EQ(math::Color(1.0, 1.0, 1.0, 1.0), msgs::Convert(res.ambient())); + + ASSERT_TRUE(res.has_background()); + EXPECT_EQ(math::Color(0.8, 0.8, 0.8, 1.0), msgs::Convert(res.background())); + + EXPECT_TRUE(res.shadows()); + EXPECT_FALSE(res.grid()); + EXPECT_FALSE(res.has_fog()); + EXPECT_FALSE(res.has_sky()); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc index 215a58b038..8bc11041d0 100644 --- a/test/integration/thruster.cc +++ b/test/integration/thruster.cc @@ -190,17 +190,20 @@ void ThrusterTest::TestWorld(const std::string &_world, pub.Publish(msg); // Check movement - for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; - ++sleep) + if (_namespace != "lowbattery") { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - fixture.Server()->Run(true, 100, false); - } - EXPECT_LT(sleep, maxSleep); - EXPECT_LT(5.0, modelPoses.back().Pos().X()); + for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; + ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + fixture.Server()->Run(true, 100, false); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_LT(5.0, modelPoses.back().Pos().X()); - EXPECT_EQ(100u * sleep, modelPoses.size()); - EXPECT_EQ(100u * sleep, propellerAngVels.size()); + EXPECT_EQ(100u * sleep, modelPoses.size()); + EXPECT_EQ(100u * sleep, propellerAngVels.size()); + } // F = m * a // s = a * t^2 / 2 @@ -209,6 +212,14 @@ void ThrusterTest::TestWorld(const std::string &_world, double xTol{1e-2}; for (unsigned int i = 0; i < modelPoses.size(); ++i) { + if (_namespace == "lowbattery" && i > 545) + { + // Battery discharged should not accelerate + EXPECT_NEAR(modelPoses[i-1].Pos().X() - modelPoses[i-2].Pos().X(), + modelPoses[i].Pos().X() - modelPoses[i-1].Pos().X(), 1e-6); + continue; + } + auto pose = modelPoses[i]; auto time = dt * i; EXPECT_NEAR(force * time * time / (2 * _mass), pose.Pos().X(), xTol); @@ -219,7 +230,7 @@ void ThrusterTest::TestWorld(const std::string &_world, // The joint velocity command adds some roll to the body which the PID // wrench doesn't - if (_namespace == "custom") + if (_namespace == "custom" || _namespace == "lowbattery") EXPECT_NEAR(0.0, pose.Rot().Roll(), 0.1); else EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); @@ -232,7 +243,14 @@ void ThrusterTest::TestWorld(const std::string &_world, // It takes a few iterations to reach the speed if (i > 25) { - EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + if (_namespace == "lowbattery" && i > 545) + { + EXPECT_NEAR(0.0, angVel.X(), _baseTol); + } + else + { + EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + } } EXPECT_NEAR(0.0, angVel.Y(), _baseTol); EXPECT_NEAR(0.0, angVel.Z(), _baseTol); @@ -292,3 +310,14 @@ TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(VelocityControl)) // Tolerance is high because the joint command disturbs the vehicle body this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); } + +///////////////////////////////////////////////// +TEST_F(ThrusterTest, IGN_UTILS_TEST_DISABLED_ON_WIN32(BatteryIntegration)) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_battery.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, "lowbattery", 0.005, 950, 0.25, 1e-2); +} + diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc new file mode 100644 index 0000000000..9f2d46a455 --- /dev/null +++ b/test/integration/triggered_camera.cc @@ -0,0 +1,109 @@ +/* + * 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. + * + */ + +#include + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "gz/sim/Server.hh" +#include "gz/sim/test_config.hh" +#include +#include +#include +#include +#include + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; +using namespace std::chrono_literals; + +/// \brief Test TriggeredCameraTest system +class TriggeredCameraTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +msgs::Image imageMsg; +unsigned char *imageBuffer = nullptr; + +///////////////////////////////////////////////// +void imageCb(const msgs::Image &_msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + mutex.lock(); + unsigned int imageSamples = _msg.width() * _msg.height() * 3; + + if (!imageBuffer) + imageBuffer = new unsigned char[imageSamples]; + memcpy(imageBuffer, _msg.data().c_str(), imageSamples); + mutex.unlock(); +} + +///////////////////////////////////////////////// +// The test checks the Triggered Camera readings +TEST_F(TriggeredCameraTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(TriggeredCameraBox)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "triggered_camera_sensor.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(100us); + server.Run(false, 0, false); + + // Subscribe to the image topic + transport::Node node; + node.Subscribe("/camera", &imageCb); + + transport::Node triggerNode; + std::string triggerTopic = + "/camera/trigger"; + + auto pub = triggerNode.Advertise(triggerTopic); + msgs::Boolean msg; + msg.set_data(true); + + int sleep{0}; + int maxSleep{30}; + while (imageBuffer == nullptr && sleep < maxSleep) + { + pub.Publish(msg); + std::this_thread::sleep_for(100ms); + sleep++; + } + EXPECT_LT(sleep, maxSleep); + ASSERT_NE(imageBuffer, nullptr); + + delete[] imageBuffer; +} diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf index ebe6784421..8c73831592 100644 --- a/test/worlds/conveyor.sdf +++ b/test/worlds/conveyor.sdf @@ -24,6 +24,8 @@ 1.0 1.0 1.0 0.8 0.8 0.8 + true + false diff --git a/test/worlds/joint_velocity_limit.sdf b/test/worlds/joint_velocity_limit.sdf new file mode 100644 index 0000000000..e31a7b27a5 --- /dev/null +++ b/test/worlds/joint_velocity_limit.sdf @@ -0,0 +1,265 @@ + + + + + 0.001 + 1.0 + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 3 0 0 0 0 0 + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + 1 + + + + + + upper_link + lower_link + + 1.0 0 0 + + 1 + + + + + + + + diff --git a/test/worlds/quadcopter_velocity_control_nested.sdf b/test/worlds/quadcopter_velocity_control_nested.sdf new file mode 100644 index 0000000000..67c4bca931 --- /dev/null +++ b/test/worlds/quadcopter_velocity_control_nested.sdf @@ -0,0 +1,452 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.053302 0 0 0 + + + 1.5 + + 0.0347563 + 0 + 0 + 0.07 + 0 + 0.0977 + + + + + + 0.30 0.42 0.11 + + + + + + + 0.15 0.21 0.11 + + + + + + 0.13 -0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_0 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_1 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + 0.13 0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_2 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 -0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_3 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + + + + 2 + + 0.00166667 + 0 + 0 + 0.00166667 + 0 + 0.00166667 + + + + 0.025 0 0 0 1.57079 0 + + + 0.05 + 0.05 + + + + + 0.025 0 0 0 1.57079 0 + + + 0.05 + 0.05 + + + + 0.05 0.05 0.05 + 0.8 0.8 0.8 + + + + + + 2 + + 0.00166667 + 0 + 0 + 0.00166667 + 0 + 0.00166667 + + + + 0.0505 0 0 0 1.57079 0 + + + 0.04 + 0.01 + + + + + 0.0505 0 0 0 1.57079 0 + + + 0.04 + 0.01 + + + + 0.02 0.02 0.02 + 0.3 0.3 0.3 + + + + + link + link_1 + + + + base_link + nested_payload::link + + + + + X3 + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 1 + velocity + + + X3 + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 1 + velocity + + + X3 + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 1 + velocity + + + X3 + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 8000.0 + 2.54858e-05 + 0.016 + gazebo/command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 1 + velocity + + + X3 + gazebo/command/twist + enable + base_link + 2.7 2.7 2.7 + 10 10 0.15 + 0.4 0.52 0.18 + 2 2 2 + + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + + diff --git a/test/worlds/thruster_battery.sdf b/test/worlds/thruster_battery.sdf new file mode 100644 index 0000000000..cb62c39220 --- /dev/null +++ b/test/worlds/thruster_battery.sdf @@ -0,0 +1,133 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + lowbattery + propeller_joint + 0.005 + 950 + 0.25 + true + 300 + 0 + + + + linear_battery + 14.4 + 14.4 + true + + 0.00005 + 400 + + 0.003064 + + 28.8 + + + + true + true + 42.1 + + + + + diff --git a/test/worlds/triggered_camera_sensor.sdf b/test/worlds/triggered_camera_sensor.sdf new file mode 100644 index 0000000000..d40902ad9d --- /dev/null +++ b/test/worlds/triggered_camera_sensor.sdf @@ -0,0 +1,57 @@ + + + + + .001 + 0 + + + + + ogre2 + 1 0 0 1 + + + + true + 0 0 1.0 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + true + camera/trigger + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + diff --git a/tutorials/python_interfaces.md b/tutorials/python_interfaces.md index e046a12e39..f1293c2f50 100644 --- a/tutorials/python_interfaces.md +++ b/tutorials/python_interfaces.md @@ -37,9 +37,7 @@ fixture.finalize() - **Step 5**: Run the server ```python -server.run(False, 1000, False) -while(server.is_running()): - time.sleep(0.1) +server.run(True, 1000, False) ``` # Run the example