diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index 8bcba178ad..c8809eb33d 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -23,6 +23,7 @@ libsdformat15-dev libtinyxml2-dev libxi-dev libxmu-dev +libpython3-dev python3-distutils python3-gz-math8 python3-gz-msgs11 diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc index 5cf7e725ba..68334cffc3 100644 --- a/examples/standalone/marker/marker.cc +++ b/examples/standalone/marker/marker.cc @@ -172,7 +172,7 @@ int main(int _argc, char **_argv) gz::msgs::Set(markerMsg.add_point(), gz::math::Vector3d(0, 0, 0.05)); double radius = 2; - for (double t = 0; t <= M_PI; t+= 0.01) + for (double t = 0; t <= GZ_PI; t+= 0.01) { gz::msgs::Set(markerMsg.add_point(), gz::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); diff --git a/examples/worlds/CMakeLists.txt b/examples/worlds/CMakeLists.txt index fee3a9e41c..258ee1529e 100644 --- a/examples/worlds/CMakeLists.txt +++ b/examples/worlds/CMakeLists.txt @@ -2,4 +2,8 @@ file(GLOB files "*.sdf") install(FILES ${files} DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) +file(GLOB csv_files "*.csv") +install(FILES ${csv_files} + DESTINATION ${GZ_DATA_INSTALL_DIR}/worlds) + add_subdirectory(thumbnails) diff --git a/examples/worlds/advanced_lift_drag_system.sdf b/examples/worlds/advanced_lift_drag_system.sdf new file mode 100644 index 0000000000..8b21318720 --- /dev/null +++ b/examples/worlds/advanced_lift_drag_system.sdf @@ -0,0 +1,686 @@ + + + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 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.246 0 0 0 + + 0 0 0 0 0 0 + + 0 0 0 0 0 0 + 1 + + 0.197563 + 0 + 0 + 0.1458929 + 0 + 0.1477 + + + + 0 0 -0.07 0 0 0 + + + 0.47 0.47 0.11 + + + + + + 10 + 0.01 + + + + + + + + + 0.07 0 -0.08 0 0 0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/body.dae + + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + + 1 + + 0 + + + 1 + 250 + + + + 1 + 50 + + + + 0 + 0.01 + + + + + + + + 0.3 0 0.0 0 1.57 0 + + 0 0 0 0 0 0 + 0.005 + + 9.75e-07 + 0 + 0 + 0.000166704 + 0 + 0.000167604 + + + + 0.0 0 0.0 0 0 0 + + + 0.005 + 0.1 + + + + + + + + + + + + + 0 0 -0.09 0 0 0 + + + 1 1 1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/iris_prop_ccw.dae + + + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + + + 1 + + 0 + + + + rotor_puller + base_link + + 1 0 0 + + -1e+16 + 1e+16 + + + 0 + 0 + + 1 + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 0.3 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/left_aileron.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 -0.3 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/right_aileron.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 0.15 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/left_flap.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + 0 -0.15 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/right_flap.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + -0.5 0 0 0.00 0 0.0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/elevators.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + + 0.00000001 + + 0.000001 + 0.0 + 0.000001 + 0.0 + 0.0 + 0.000001 + + -0.5 0 0.05 0 0 0 + + + 0.07 0.0 -0.08 0.00 0 0.0 + + + 0.1 0.1 0.1 + https://fuel.gazebosim.org/1.0/frede791/models/Advanced%20Plane/tip/files/meshes/rudder.dae + + + + 1 0 0 1.0 + 1 0 0 1.0 + + + + + + base_link + left_elevon + -0.07 0.4 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_0 + servo_0 + 10 + 0 + 0 + + + + base_link + right_elevon + -0.07 -0.4 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_1 + servo_1 + 10 + 0 + 0 + + + + base_link + left_flap + -0.07 0.2 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_4 + servo_4 + 10 + 0 + 0 + + + + base_link + right_flap + -0.07 -0.2 0.08 0.00 0 0.0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_5 + servo_5 + 10 + 0 + 0 + + + + base_link + elevator + -0.5 0 0 0 0 0 + + 0 1 0 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_2 + servo_2 + 10 + 0 + 0 + + + + base_link + rudder + -0.5 0 0.05 0.00 0 0.0 + + 0 0 1 + + + -0.53 + 0.53 + + + 1.000 + + + + + 1 + + + + + + servo_3 + servo_3 + 10 + 0 + 0 + + + + + + + 0.0 + 0.15188 + 6.5 + 0.97 + 5.015 + 0.029 + 0.075 + -0.463966 + -0.258244 + -0.039250 + 0.100826 + 0.0 + 0.065861 + 0.0 + -0.487407 + 0.0 + -0.040416 + 0.055166 + 0.0 + 7.971792 + 0.0 + -12.140140 + 0.0 + 0.0 + 0.230299 + 0.0 + 0.078165 + 0.0 + -0.089947 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -0.12 0.0 0.0 + 0.34 + 0.22 + 1.2041 + 1 0 0 + 0 0 1 + base_link + 4 + + servo_0 + 0 + 1 + -0.000059 + 0.000171 + -0.011940 + -0.003331 + 0.001498 + -0.000057 + + + servo_1 + 1 + 1 + -0.000059 + -0.000171 + -0.011940 + 0.003331 + 0.001498 + 0.000057 + + + servo_2 + -1 + 2 + 0.000274 + 0 + 0.010696 + 0.0 + -0.025798 + 0.0 + + + servo_3 + 1 + 3 + 0.0 + -0.003913 + 0.0 + -0.000257 + 0.0 + 0.001613 + + + + + rotor_puller_joint + rotor_puller + cw + 0.0125 + 0.025 + 3500 + 8.54858e-06 + 0.01 + command/motor_speed + 0 + 8.06428e-05 + 1e-06 + 10 + velocity + + + servo_0 + + + + diff --git a/examples/worlds/elevator.sdf b/examples/worlds/elevator.sdf index f50ee17c59..0cbcd8702e 100644 --- a/examples/worlds/elevator.sdf +++ b/examples/worlds/elevator.sdf @@ -10,6 +10,12 @@ gz topic -e -t /model/elevator/state + Note that when commanding the lift to the ground floor: + + gz topic -t "/model/elevator/cmd" -m gz.msgs.Int32 -p "data: 0" + + The output of the topic echo command will stop as protobuf does not + distinguish between the un-set value and zero for integer fields. --> diff --git a/examples/worlds/environmental_sensor.sdf b/examples/worlds/environmental_sensor.sdf index e1947c8591..8ef8ed35f1 100644 --- a/examples/worlds/environmental_sensor.sdf +++ b/examples/worlds/environmental_sensor.sdf @@ -1,5 +1,12 @@ - + + @@ -104,6 +112,7 @@ 1 30 + sensors/humidity diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index d67a17d4de..842e287025 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -173,6 +174,13 @@ namespace gz public: std::optional WorldPose( const EntityComponentManager &_ecm) const; + /// \brief Get the world inertia of the link. + /// \param[in] _ecm Entity-component manager. + /// \return Inertia of the link pose in world frame or nullopt + /// if the link does not have the component components::Inertial. + public: std::optional WorldInertial( + const EntityComponentManager &_ecm) const; + /// \brief Get the world pose of the link inertia. /// \param[in] _ecm Entity-component manager. /// \return Inertial pose in world frame or nullopt if the diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 51a6b15807..1c8bb8381d 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -79,6 +79,9 @@ void defineSimLink(py::object module) .def("world_pose", &gz::sim::Link::WorldPose, py::arg("ecm"), "Get the pose of the link frame in the world coordinate frame.") + .def("world_inertial", &gz::sim::Link::WorldInertial, + py::arg("ecm"), + "Get the inertia of the link in the world frame.") .def("world_inertial_pose", &gz::sim::Link::WorldInertialPose, py::arg("ecm"), "Get the world pose of the link inertia.") diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 35071869e1..4d6be57631 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -18,7 +18,7 @@ from gz_test_deps.common import set_verbosity from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity -from gz_test_deps.math import Matrix3d, Vector3d, Pose3d +from gz_test_deps.math import Inertiald, Matrix3d, Vector3d, Pose3d class TestModel(unittest.TestCase): post_iterations = 0 @@ -59,6 +59,10 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) + # World Inertial Test + self.assertEqual(Pose3d(), link.world_inertial(_ecm).pose()) + self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertial(_ecm).moi()) + self.assertEqual(10.0, link.world_inertial(_ecm).mass_matrix().mass()) # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test @@ -76,7 +80,7 @@ def on_pre_udpate_cb(_info, _ecm): self.assertEqual(Vector3d(), link.world_linear_acceleration(_ecm)) self.assertEqual(Vector3d(), link.world_angular_acceleration(_ecm)) # World Inertia Matrix Test - self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertia_matrix(_ecm)) + self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertia_matrix(_ecm)) # World Kinetic Energy Test self.assertEqual(0, link.world_kinetic_energy(_ecm)) link.enable_velocity_checks(_ecm, False) diff --git a/src/Link.cc b/src/Link.cc index 426c57e780..4fef211baf 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -190,6 +189,23 @@ std::optional Link::WorldPose( .value_or(sim::worldPose(this->dataPtr->id, _ecm)); } +////////////////////////////////////////////////// +std::optional Link::WorldInertial( + const EntityComponentManager &_ecm) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); + + if (!inertial) + return std::nullopt; + + const math::Pose3d &comWorldPose = + worldPose * inertial->Data().Pose(); + return std::make_optional( + math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); +} + ////////////////////////////////////////////////// std::optional Link::WorldInertialPose( const EntityComponentManager &_ecm) const diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 4f628a3906..4ee08be76a 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -80,6 +80,9 @@ struct MaybeGilScopedRelease #else struct MaybeGilScopedRelease { + // The empty constructor is needed to avoid an "unused variable" warning + // when an instance of this class is used. + MaybeGilScopedRelease(){} }; #endif } diff --git a/src/gui/plugins/environment_loader/EnvironmentLoader.cc b/src/gui/plugins/environment_loader/EnvironmentLoader.cc index 6ac8b464cf..cb688b3a8a 100644 --- a/src/gui/plugins/environment_loader/EnvironmentLoader.cc +++ b/src/gui/plugins/environment_loader/EnvironmentLoader.cc @@ -19,10 +19,11 @@ #include #include -#include #include +#include #include +#include #include #include @@ -33,6 +34,11 @@ #include #include +#include + +#include +#include + using namespace gz; using namespace sim; @@ -42,6 +48,11 @@ namespace sim { inline namespace GZ_SIM_VERSION_NAMESPACE { +const char* preload_plugin_name{ + "gz::sim::systems::EnvironmentPreload"}; +const char* preload_plugin_filename{ + "gz-sim-environment-preload-system"}; +using Units = msgs::DataLoadPathOptions_DataAngularUnits; /// \brief Private data class for EnvironmentLoader class EnvironmentLoaderPrivate { @@ -65,7 +76,7 @@ class EnvironmentLoaderPrivate public: int zIndex{-1}; /// \brief Index of data dimension to be used as units. - public: QString unit; + public: QString unit{"radians"}; public: using ReferenceT = math::SphericalCoordinates::CoordinateType; @@ -76,12 +87,12 @@ class EnvironmentLoaderPrivate {QString("ecef"), math::SphericalCoordinates::ECEF}}; /// \brief Map of supported spatial units. - public: const QMap + public: const QMap unitMap{ {QString("degree"), - components::EnvironmentalData::ReferenceUnits::DEGREES}, + Units::DataLoadPathOptions_DataAngularUnits_DEGREES}, {QString("radians"), - components::EnvironmentalData::ReferenceUnits::RADIANS} + Units::DataLoadPathOptions_DataAngularUnits_RADIANS} }; /// \brief Spatial reference. @@ -92,6 +103,12 @@ class EnvironmentLoaderPrivate /// \brief Whether to attempt an environmental data load. public: std::atomic needsLoad{false}; + + /// \brief Gz transport node + public: transport::Node node; + + /// \brief publisher + public: std::optional pub{std::nullopt}; }; } } @@ -123,38 +140,54 @@ void EnvironmentLoader::LoadConfig(const tinyxml2::XMLElement *) void EnvironmentLoader::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - if (this->dataPtr->needsLoad) + auto world = worldEntity(_ecm); + + if (!this->dataPtr->pub.has_value()) { - std::lock_guard lock(this->dataPtr->mutex); - this->dataPtr->needsLoad = false; - - /// TODO(arjo): Handle the case where we are loading a file in windows. - /// Should SDFormat files support going from windows <=> unix paths? - std::ifstream dataFile(this->dataPtr->dataPath.toStdString()); - gzmsg << "Loading environmental data from " - << this->dataPtr->dataPath.toStdString() - << std::endl; - try + auto topic = transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/" + "environment"); + this->dataPtr->pub = + {this->dataPtr->node.Advertise(topic)}; + } + + static bool warned = false; + if (!this->dataPtr->pub->HasConnections() && !warned) + { + warned = true; + gzwarn << "Could not find a subscriber for the environment. " + << "Attempting to load environmental preload plugin." + << std::endl; + + auto nameComp = _ecm.Component(world); + if (nullptr == nameComp) { + gzerr << "Failed to get world name" << std::endl; + return; + } + auto worldName = nameComp->Data(); + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(world); + auto plugin = req.add_plugins(); + plugin->set_name(preload_plugin_name); + plugin->set_filename(preload_plugin_filename); + plugin->set_innerxml(""); + msgs::Boolean res; + bool result; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } + + if (this->dataPtr->node.Request(service, req, timeout, res, result)) { - using ComponentDataT = components::EnvironmentalData; - auto data = ComponentDataT::MakeShared( - common::IO::ReadFrom( - common::CSVIStreamIterator(dataFile), - common::CSVIStreamIterator(), - this->dataPtr->timeIndex, { - static_cast(this->dataPtr->xIndex), - static_cast(this->dataPtr->yIndex), - static_cast(this->dataPtr->zIndex)}), - this->dataPtr->referenceMap[this->dataPtr->reference], - this->dataPtr->unitMap[this->dataPtr->unit]); - - using ComponentT = components::Environment; - _ecm.CreateComponent(worldEntity(_ecm), ComponentT{std::move(data)}); + gzdbg << "Added plugin successfully" << std::endl; } - catch (const std::invalid_argument &exc) + else { - gzerr << "Failed to load environmental data" << std::endl - << exc.what() << std::endl; + gzerr << "Failed to load plugin" << std::endl; } } } @@ -162,7 +195,25 @@ void EnvironmentLoader::Update(const UpdateInfo &, ///////////////////////////////////////////////// void EnvironmentLoader::ScheduleLoad() { - this->dataPtr->needsLoad = this->IsConfigured(); + if(this->IsConfigured() && this->dataPtr->pub.has_value()) + { + msgs::DataLoadPathOptions data; + data.set_path(this->dataPtr->dataPath.toStdString()); + data.set_time( + this->dataPtr->dimensionList[this->dataPtr->timeIndex].toStdString()); + data.set_x( + this->dataPtr->dimensionList[this->dataPtr->xIndex].toStdString()); + data.set_y( + this->dataPtr->dimensionList[this->dataPtr->yIndex].toStdString()); + data.set_z( + this->dataPtr->dimensionList[this->dataPtr->zIndex].toStdString()); + auto referenceFrame = this->dataPtr->referenceMap[this->dataPtr->reference]; + + data.set_coordinate_type(msgs::ConvertCoord(referenceFrame)); + data.set_units(this->dataPtr->unitMap[this->dataPtr->unit]); + + this->dataPtr->pub->Publish(data); + } } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc index 3ed5f6f9ce..c9c040f454 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.cc @@ -36,9 +36,7 @@ #include -#include -#include -#include +#include #include using namespace gz; @@ -51,201 +49,44 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Private data class for EnvironmentVisualization -class EnvironmentVisualizationPrivate +class EnvironmentVisualizationTool { - public: EnvironmentVisualizationPrivate() - { - this->pcPub = - this->node.Advertise("/point_cloud"); - } - /// \brief To synchronize member access. - public: std::mutex mutex; - - /// \brief first load we need to scan for existing data sensor - public: bool first {true}; - - public: std::atomic resample{true}; ///////////////////////////////////////////////// - public: void CreatePointCloudTopics( - std::shared_ptr data) { - this->pubs.clear(); - this->sessions.clear(); - for (auto key : data->frame.Keys()) - { - this->pubs.emplace(key, node.Advertise(key)); - gz::msgs::Float_V msg; - this->floatFields.emplace(key, msg); - this->sessions.emplace(key, data->frame[key].CreateSession()); - } - } - - ///////////////////////////////////////////////// - public: void Step( - const UpdateInfo &_info, - std::shared_ptr &data, - const EntityComponentManager& _ecm, - double xSamples, double ySamples, double zSamples) + public: void Initiallize( + const EntityComponentManager &_ecm) { - auto now = std::chrono::steady_clock::now(); - std::chrono::duration dt(now - this->lastTick); - - if (this->resample) - { - this->CreatePointCloudTopics(data); - this->ResizeCloud(data, _ecm, xSamples, ySamples, zSamples); - this->resample = false; - this->lastTick = now; - } - - for (auto &it : this->sessions) - { - auto res = - data->frame[it.first].StepTo(it.second, - std::chrono::duration(_info.simTime).count()); - if (res.has_value()) - { - it.second = res.value(); - } - } - - // Publish at 2 hz for now. In future make reconfigureable. - if (dt.count() > 0.5) - { - this->Visualize(data, xSamples, ySamples, zSamples); - this->Publish(); - lastTick = now; - } + auto world = worldEntity(_ecm); + auto topic = + common::joinPaths( + scopedName(world, _ecm), "environment", "visualize", "res"); + std::lock_guard lock(this->mutex); + this->pcPub = node.Advertise(topic); } ///////////////////////////////////////////////// public: void Visualize( - std::shared_ptr data, - double xSamples, double ySamples, double zSamples) { - - for (auto key : data->frame.Keys()) - { - const auto session = this->sessions[key]; - auto frame = data->frame[key]; - auto [lower_bound, upper_bound] = frame.Bounds(session); - auto step = upper_bound - lower_bound; - auto dx = step.X() / xSamples; - auto dy = step.Y() / ySamples; - auto dz = step.Z() / zSamples; - std::size_t idx = 0; - for (std::size_t x_steps = 0; x_steps < ceil(xSamples); x_steps++) - { - auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ceil(ySamples); y_steps++) - { - auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < ceil(zSamples); z_steps++) - { - auto z = lower_bound.Z() + z_steps * dz; - auto res = frame.LookUp(session, math::Vector3d(x, y, z)); - - if (res.has_value()) - { - this->floatFields[key].mutable_data()->Set(idx, - static_cast(res.value())); - } - else - { - this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); - } - idx++; - } - } - } - } - } - - ///////////////////////////////////////////////// - public: void Publish() + double xSamples, double ySamples, double zSamples) { - pcPub.Publish(this->pcMsg); - for(auto &[key, pub] : this->pubs) - { - pub.Publish(this->floatFields[key]); - } + std::lock_guard lock(this->mutex); + this->vec = msgs::Convert(math::Vector3d(xSamples, ySamples, zSamples)); + this->pcPub.Publish(vec); } - ///////////////////////////////////////////////// - public: void ResizeCloud( - std::shared_ptr data, - const EntityComponentManager& _ecm, - unsigned int xSamples, unsigned int ySamples, unsigned int zSamples) - { - assert (pubs.size() > 0); - - // Assume all data have same point cloud. - gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, - {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - auto numberOfPoints = xSamples * ySamples * zSamples; - std::size_t dataSize{ - static_cast(numberOfPoints * pcMsg.point_step())}; - pcMsg.mutable_data()->resize(dataSize); - pcMsg.set_height(1); - pcMsg.set_width(numberOfPoints); - - auto session = this->sessions[this->pubs.begin()->first]; - auto frame = data->frame[this->pubs.begin()->first]; - auto [lower_bound, upper_bound] = - frame.Bounds(session); + /// \brief The sample resolution + public: gz::msgs::Vector3d vec; - auto step = upper_bound - lower_bound; - auto dx = step.X() / xSamples; - auto dy = step.Y() / ySamples; - auto dz = step.Z() / zSamples; - - // Populate point cloud - gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); - gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); - gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); - - for (std::size_t x_steps = 0; x_steps < xSamples; x_steps++) - { - auto x = lower_bound.X() + x_steps * dx; - for (std::size_t y_steps = 0; y_steps < ySamples; y_steps++) - { - auto y = lower_bound.Y() + y_steps * dy; - for (std::size_t z_steps = 0; z_steps < zSamples; z_steps++) - { - auto z = lower_bound.Z() + z_steps * dz; - auto coords = getGridFieldCoordinates( - _ecm, math::Vector3d{x, y, z}, - data); + /// \brief Publisher to publish sample resolution + public: transport::Node::Publisher pcPub; - if (!coords.has_value()) - { - continue; - } + /// \brief Gz transport node + public: transport::Node node; - auto pos = coords.value(); - *xIter = pos.X(); - *yIter = pos.Y(); - *zIter = pos.Z(); - ++xIter; - ++yIter; - ++zIter; - } - } - } - for (auto key : data->frame.Keys()) - { - this->floatFields[key].mutable_data()->Resize( - numberOfPoints, std::nanf("")); - } - } + /// \brief To synchronize member access. + public: std::mutex mutex; - public: transport::Node::Publisher pcPub; - public: std::unordered_map pubs; - public: std::unordered_map floatFields; - public: transport::Node node; - public: gz::msgs::PointCloudPacked pcMsg; - public: std::unordered_map> sessions; - public: std::chrono::time_point lastTick; + /// \brief first load we need to scan for existing data sensor + public: bool first{true}; }; } } @@ -253,10 +94,14 @@ class EnvironmentVisualizationPrivate ///////////////////////////////////////////////// EnvironmentVisualization::EnvironmentVisualization() - : GuiSystem(), dataPtr(new EnvironmentVisualizationPrivate) + : GuiSystem(), dataPtr(new EnvironmentVisualizationTool) { gui::App()->Engine()->rootContext()->setContextProperty( "EnvironmentVisualization", this); + this->qtimer = new QTimer(this); + connect(qtimer, &QTimer::timeout, + this, &EnvironmentVisualization::ResamplePointcloud); + this->qtimer->start(1000); } ///////////////////////////////////////////////// @@ -274,38 +119,23 @@ void EnvironmentVisualization::LoadConfig(const tinyxml2::XMLElement *) } ///////////////////////////////////////////////// -void EnvironmentVisualization::Update(const UpdateInfo &_info, +void EnvironmentVisualization::Update(const UpdateInfo &, EntityComponentManager &_ecm) { - _ecm.EachNew( - [this]( - const Entity &/*_entity*/, - const components::Environment* /*environment*/ - ) { - this->dataPtr->resample = true; - return true; - } - ); - - auto environData = - _ecm.Component( - worldEntity(_ecm)); - - if (environData == nullptr) + if (this->dataPtr->first) { - return; + this->dataPtr->Initiallize(_ecm); + this->dataPtr->first = false; + this->ResamplePointcloud(); } - - this->dataPtr->Step( - _info, environData->Data(), _ecm, - this->xSamples, this->ySamples, this->zSamples - ); } ///////////////////////////////////////////////// void EnvironmentVisualization::ResamplePointcloud() { - this->dataPtr->resample = true; + this->dataPtr->Visualize( + this->xSamples, this->ySamples, this->zSamples + ); } diff --git a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh index 91cd420eb9..e5a3eb4397 100644 --- a/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh +++ b/src/gui/plugins/environment_visualization/EnvironmentVisualization.hh @@ -30,7 +30,7 @@ namespace sim // Inline bracket to help doxygen filtering. inline namespace GZ_SIM_VERSION_NAMESPACE { - class EnvironmentVisualizationPrivate; + class EnvironmentVisualizationTool; /// \class EnvironmentVisualization EnvironmentVisualization.hh /// gz/sim/systems/EnvironmentVisualization.hh @@ -66,13 +66,15 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \internal /// \brief Pointer to private data - private: std::unique_ptr dataPtr; + private: std::unique_ptr dataPtr; public: unsigned int xSamples{10}; public: unsigned int ySamples{10}; public: unsigned int zSamples{10}; + + private: QTimer* qtimer; }; } } diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 7eb7786908..d75a20663d 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -96,6 +96,7 @@ endfunction() add_subdirectory(ackermann_steering) add_subdirectory(acoustic_comms) +add_subdirectory(advanced_lift_drag) add_subdirectory(air_pressure) add_subdirectory(air_speed) add_subdirectory(altimeter) diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index f82e0cec20..3d829d8902 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -36,106 +36,106 @@ namespace systems /// \brief Ackermann steering controller which can be attached to a model /// with any number of left and right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Boolean used to only control the steering angle + /// - ``: Boolean used to only control the steering angle /// only. Calculates the angles of wheels from steering_limit, wheel_base, /// and wheel_separation. Uses gz::msg::Double on default topic name /// `/model/{name_of_model}/steer_angle`. Automatically set True when /// `` is True, uses defaults topic name "/actuators". /// - /// `` True to enable the use of actutor message + /// - `` True to enable the use of actutor message /// for steering angle command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the steering angle position actuator. /// - /// ``: Float used to control the steering angle P gain. + /// - ``: Float used to control the steering angle P gain. /// Only used for when in steering_only mode. /// - /// ``: Name of a joint that controls a left wheel. This + /// - ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a right wheel. This + /// - ``: Name of a joint that controls a right wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls steering for - /// left wheel. This element must appear once. Vehicles that steer - /// rear wheels are not currently correctly supported. + /// - ``: Name of a joint that controls steering for + /// left wheel. This element must appear once. Vehicles that steer + /// rear wheels are not currently correctly supported. /// - /// ``: Name of a joint that controls steering for - /// right wheel. This element must appear once. + /// - ``: Name of a joint that controls steering for + /// right wheel. This element must appear once. /// - /// ``: Distance between wheels, in meters. This element + /// - ``: Distance between wheels, in meters. This element /// is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Distance between wheel kingpins, in meters. This + /// - ``: Distance between wheel kingpins, in meters. This /// element is optional, although it is recommended to be included with an /// appropriate value. The default value is 0.8m. Generally a bit smaller /// than the wheel_separation. /// - /// ``: Distance between front and rear wheels, in meters. This + /// - ``: Distance between front and rear wheels, in meters. This /// element is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Value to limit steering to. Can be calculated by + /// - ``: Value to limit steering to. Can be calculated by /// measuring the turning radius and wheel_base of the real vehicle. /// steering_limit = asin(wheel_base / turning_radius) /// The default value is 0.5 radians. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Minimum velocity [m/s], usually <= 0. - /// ``: Maximum velocity [m/s], usually >= 0. - /// ``: Minimum acceleration [m/s^2], usually <= 0. - /// ``: Maximum acceleration [m/s^2], usually >= 0. - /// ``: jerk [m/s^3], usually <= 0. - /// ``: jerk [m/s^3], usually >= 0. + /// - ``: Minimum velocity [m/s], usually <= 0. + /// - ``: Maximum velocity [m/s], usually >= 0. + /// - ``: Minimum acceleration [m/s^2], usually <= 0. + /// - ``: Maximum acceleration [m/s^2], usually >= 0. + /// - ``: jerk [m/s^3], usually <= 0. + /// - ``: jerk [m/s^3], usually >= 0. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command messages. This element is optional, and the /// default value is `/model/{name_of_model}/cmd_vel` or when steering_only /// is true `/model/{name_of_model}/steer_angle`. /// - /// ``: Custom sub_topic that this system will subscribe to in + /// - ``: Custom sub_topic that this system will subscribe to in /// order to receive command messages. This element is optional, and /// creates a topic `/model/{name_of_model}/{sub_topic}` /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element is optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry transform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element is optional, /// and the default value is `{name_of_model}/{name_of_link}`. /// /// A robot with rear drive and front steering would have one each /// of left_joint, right_joint, left_steering_joint and - /// right_steering_joint + /// right_steering_joint. /// /// References: - /// https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering - /// https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf - /// https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ + /// - https://github.com/gazebosim/gz-sim/tree/main/src/systems/ackermann_steering + /// - https://www.auto.tuwien.ac.at/bib/pdf_TR/TR0183.pdf + /// - https://github.com/froohoo/ackermansteer/blob/master/ackermansteer/ class AckermannSteering diff --git a/src/systems/acoustic_comms/AcousticComms.hh b/src/systems/acoustic_comms/AcousticComms.hh index 2845436cef..45e7db8845 100644 --- a/src/systems/acoustic_comms/AcousticComms.hh +++ b/src/systems/acoustic_comms/AcousticComms.hh @@ -76,6 +76,7 @@ namespace systems /// * : Seed value to be used for random sampling. /// /// Here's an example: + /// ``` /// @@ -93,6 +94,7 @@ namespace systems /// /// /// + /// ``` class AcousticComms: public gz::sim::comms::ICommsModel diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc new file mode 100644 index 0000000000..a7c60109fd --- /dev/null +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.cc @@ -0,0 +1,852 @@ +/* + * 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. + * + * @author: Karthik Srivatsan, Frederik Markus + * @version: 1.1 + * + * @brief: this plugin models the lift and drag of an aircraft + * as a single body, using stability, aerodynamic, and control derivatives. + * It takes in a specified number of control surfaces and control + * derivatives are defined/specified with respect to the deflection + * of individual control surfaces. Coefficients for this plugin can be + * obtained using Athena Vortex Lattice (AVL) by Mark Drela + * https://nps.edu/web/adsc/aircraft-aerodynamics2 + * The sign conventions used in this plugin are therefore written + * in a way to be compatible with AVL. + * Force equations are computed in the body, while + * moment equations are computed in the stability frame. + * Has been adapted for Gazebo (Ignition) using the ECS. + * + * + */ + +#include "AdvancedLiftDrag.hh" + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" + +#include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/Inertial.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/Link.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ExternalWorldWrenchCmd.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +class gz::sim::systems::AdvancedLiftDragPrivate +{ + // Initialize the system + public: void Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf); + + /// \brief Initializes lift and drag forces in order to later + /// update the corresponding components + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Update(EntityComponentManager &_ecm); + + /// \brief Compute Control Surface effects + /// \param[in] _ecm Immutable reference to the EntityComponentManager + public: void Comp_Ctrl_surf_eff(EntityComponentManager &_ecm); + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Lift Coefficient at zero angle of attack + public: double CL0 = 0.0; + + /// \brief Drag coefficient at zero angle of attack + public: double CD0 = 0.0; + + /// \brief Pitching moment coefficient at zero angle of attack + public: double Cem0 = 0.0; + + // Get angle-of-attack (alpha) derivatives + /// \brief dCL/da (slope of CL-alpha curve) + public: double CLa = 0.0; + + /// \brief dCy/da (sideforce slope wrt alpha) + public: double CYa = 0.0; + + /// \brief dCl/da (roll moment slope wrt alpha) + public: double Cella = 0.0; + + /// \brief dCm/da (pitching moment slope wrt alpha - before stall) + public: double Cema = 0.0; + + /// \brief dCn/da (yaw moment slope wrt alpha) + public: double Cena = 0.0; + + // Get sideslip angle (beta) derivatives + /// \brief dCL/dbeta (lift coefficient slope wrt beta) + public: double CLb = 0.0; + + /// \brief dCY/dbeta (side force slope wrt beta) + public: double CYb = 0.0; + + /// \brief dCl/dbeta (roll moment slope wrt beta) + public: double Cellb = 0.0; + + /// \brief dCm/dbeta (pitching moment slope wrt beta) + public: double Cemb = 0.0; + + /// \brief dCn/dbeta (yaw moment slope wrt beta) + public: double Cenb = 0.0; + + /// \brief angle of attack when airfoil stalls + public: double alphaStall = GZ_PI_2; + + /// \brief The angle of attack + public: double alpha = 0.0; + + /// \brief The sideslip angle + public: double beta = 0.0; + + /// \brief Slope of the Cm-alpha curve after stall + public: double CemaStall = 0.0; + + /// \brief AVL reference point (it replaces the center of pressure + /// in the original LiftDragPlugin) + public: gz::math::Vector3d cp = math::Vector3d::Zero; + + // Get the derivatives with respect to non-dimensional rates. + // In the next few lines, if you see "roll/pitch/yaw rate", remember it is in + // a non-dimensional form- it is not the actual body rate. + // Also, keep in mind that this CDp is not parasitic drag: that is CD0. + + /// \brief dCD/dp (drag coefficient slope wrt roll rate) + public: double CDp = 0.0; + + /// \brief dCY/dp (sideforce slope wrt roll rate) + public: double CYp = 0.0; + + /// \brief dCL/dp (lift coefficient slope wrt roll rate) + public: double CLp = 0.0; + + /// \brief dCl/dp (roll moment slope wrt roll rate) + public: double Cellp = 0.0; + + /// \brief dCm/dp (pitching moment slope wrt roll rate) + public: double Cemp = 0.0; + + /// \brief dCn/dp (yaw moment slope wrt roll rate) + public: double Cenp = 0.0; + + /// \brief dCD/dq (drag coefficient slope wrt pitching rate) + public: double CDq = 0.0; + + /// \brief dCY/dq (side force slope wrt pitching rate) + public: double CYq = 0.0; + + /// \brief dCL/dq (lift coefficient slope wrt pitching rate) + public: double CLq = 0.0; + + /// \brief dCl/dq (roll moment slope wrt pitching rate) + public: double Cellq = 0.0; + + /// \brief dCm/dq (pitching moment slope wrt pitching rate) + public: double Cemq = 0.0; + + /// \brief dCn/dq (yaw moment slope wrt pitching rate) + public: double Cenq = 0.0; + + /// \brief dCD/dr (drag coefficient slope wrt yaw rate) + public: double CDr = 0.0; + + /// \brief dCY/dr (side force slope wrt yaw rate) + public: double CYr = 0.0; + + /// \brief dCL/dr (lift coefficient slope wrt yaw rate) + public: double CLr = 0.0; + + /// \brief dCl/dr (roll moment slope wrt yaw rate) + public: double Cellr = 0.0; + + /// \brief dCm/dr (pitching moment slope wrt yaw rate) + public: double Cemr = 0.0; + + /// \brief dCn/dr (yaw moment slope wrt yaw rate) + public: double Cenr = 0.0; + + /// \brief Number of present control surfaces + public: int num_ctrl_surfaces = 0; + + /// Initialize storage of control surface properties + /// \brief Joint that the control surface connects to + public: std::vector controlJoints; + + /// \brief Direction the control surface deflects to + public: std::vector ctrl_surface_direction; + + /// \brief Effect of the control surface's deflection on drag + public: std::vector CD_ctrl; + + /// \brief Effect of the control surface's deflection on side force + public: std::vector CY_ctrl; + + /// \brief Effect of the control surface's deflection on lift + public: std::vector CL_ctrl; + + /// \brief Effect of the control surface's deflection on roll moment + public: std::vector Cell_ctrl; + + /// \brief Effect of the control surface's deflection on pitching moment + public: std::vector Cem_ctrl; + + /// \brief Effect of the control surface's deflection on yaw moment + public: std::vector Cen_ctrl; + + /// \brief Add aspect ratio (should that be computed?) + public: double AR = 0.0; + + /// \brief Add mean-aerodynamic chord + public: double mac = 0.0; + + /// \brief Add wing efficiency (Oswald efficiency factor for a 3D wing) + public: double eff = 0.0; + + /// \brief The sigmoid blending parameter + public: double M = 15.0; + + /// \brief coefficients for the flat plate drag model + public: double CD_fp_k1 = -0.224; + public: double CD_fp_k2 = -0.115; + + + /// \brief air density + /// at 25 deg C it's about 1.1839 kg/m^3 + /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. + public: double rho = 1.2041; + + /// \brief if the shape is aerodynamically radially symmetric about + /// the forward direction. Defaults to false for wing shapes. + /// If set to true, the upward direction is determined by the + /// angle of attack. + public: bool radialSymmetry = false; + + /// \brief effective planeform surface area + public: double area = 1.0; + + /// \brief Normally, this is taken as a direction parallel to the chord + /// of the airfoil in zero angle of attack forward flight. + public: math::Vector3d forward = math::Vector3d::UnitX; + + /// \brief A vector in the lift/drag plane, perpendicular to the forward + /// vector. Inflow velocity orthogonal to forward and upward vectors + /// is considered flow in the wing sweep direction. + public: math::Vector3d upward = math::Vector3d::UnitZ; + + /// \brief how much to change CL per radian of control surface joint + /// value. + public: double controlJointRadToCL = 4.0; + + /// \brief Link entity targeted this plugin. + public: Entity linkEntity; + + /// \brief Joint entity that actuates a control surface for this lifting body + public: Entity controlJointEntity; + + /// \brief Set during Load to true if the configuration for the system is + /// valid and the post-update can run + public: bool validConfig{false}; + + /// \brief Copy of the sdf configuration used for this plugin + public: sdf::ElementPtr sdfConfig; + + /// \brief Initialization flag + public: bool initialized{false}; +}; + +////////////////////////////////////////////////// +void AdvancedLiftDragPrivate::Load(const EntityComponentManager &_ecm, + const sdf::ElementPtr &_sdf) +{ + this->CL0 = _sdf->Get("CL0", this->CL0).first; + this->CD0 = _sdf->Get("CD0", this->CD0).first; + this->Cem0 = _sdf->Get("Cem0", this->Cem0).first; + this->CLa = _sdf->Get("CLa", this->CLa).first; + this->CYa = _sdf->Get("CYa", this->CYa).first; + this->Cella = _sdf->Get("Cella", this->Cella).first; + this->Cema = _sdf->Get("Cema", this->Cema).first; + this->Cena = _sdf->Get("Cena", this->Cena).first; + this->CLb = _sdf->Get("CLb", this->CLb).first; + this->CYb = _sdf->Get("CYb", this->CYb).first; + this->Cellb = _sdf->Get("Cellb", this->Cellb).first; + this->Cemb = _sdf->Get("Cemb", this->Cemb).first; + this->Cenb = _sdf->Get("Cenb", this->Cenb).first; + this->alphaStall = _sdf->Get("alpha_stall", this->alphaStall).first; + this->CemaStall = _sdf->Get("Cema_stall", this->CemaStall).first; + this->cp = _sdf->Get("cp", this->cp).first; + this->CDp = _sdf->Get("CDp", this->CDp).first; + this->CYp = _sdf->Get("CYp", this->CYp).first; + this->CLp = _sdf->Get("CLp", this->CLp).first; + this->Cellp = _sdf->Get("Cellp", this->Cellp).first; + this->Cemp = _sdf->Get("Cemp", this->Cemp).first; + this->Cenp = _sdf->Get("Cenp", this->Cenp).first; + + this->CDq = _sdf->Get("CDq", this->CDq).first; + this->CYq = _sdf->Get("CYq", this->CYq).first; + this->CLq = _sdf->Get("CLq", this->CLq).first; + this->Cellq = _sdf->Get("Cellq", this->Cellq).first; + this->Cemq = _sdf->Get("Cemq", this->Cemq).first; + this->Cenq = _sdf->Get("Cenq", this->Cenq).first; + this->CDr = _sdf->Get("CDr", this->CDr).first; + this->CYr = _sdf->Get("CYr", this->CYr).first; + this->CLr = _sdf->Get("CLr", this->CLr).first; + this->Cellr = _sdf->Get("Cellr", this->Cellr).first; + this->Cemr = _sdf->Get("Cemr", this->Cemr).first; + this->Cenr = _sdf->Get("Cenr", this->Cenr).first; + this->num_ctrl_surfaces = _sdf->Get( + "num_ctrl_surfaces", this->num_ctrl_surfaces).first; + + /* + Next, get the properties of each control surface: which joint it connects to, + which direction it deflects in, and the effect of its deflection on the + coefficient of drag, side force, lift, roll moment, pitching moment, and yaw moment. + */ + + while( _sdf->HasElement("control_surface") ) + { + auto curr_ctrl_surface = _sdf->GetElement("control_surface"); + auto ctrl_surface_name = curr_ctrl_surface->GetElement( + "name")->Get(); + auto entities = + entitiesFromScopedName(ctrl_surface_name, _ecm, this->model.Entity()); + + if (entities.empty()) + { + gzerr << "Joint with name[" << ctrl_surface_name << "] not found. " + << "The LiftDrag will not generate forces.\n"; + this->validConfig = false; + return; + } + else if (entities.size() > 1) + { + gzwarn << "Multiple joint entities with name[" << ctrl_surface_name + << "] found. Using the first one.\n"; + } + + controlJointEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->controlJointEntity, + components::Joint::typeId)) + { + this->controlJointEntity = kNullEntity; + gzerr << "Entity with name[" << ctrl_surface_name + << "] is not a joint.\n"; + this->validConfig = false; + return; + } + + this->controlJoints.push_back(controlJointEntity); + this->ctrl_surface_direction.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "direction"))->GetValue())->GetAsString())); + this->CD_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CD_ctrl"))->GetValue())->GetAsString())); + this->CY_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CY_ctrl"))->GetValue())->GetAsString())); + this->CL_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "CL_ctrl"))->GetValue())->GetAsString())); + this->Cell_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cell_ctrl"))->GetValue())->GetAsString())); + this->Cem_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cem_ctrl"))->GetValue())->GetAsString())); + this->Cen_ctrl.push_back( + std::stod(((curr_ctrl_surface->GetElement( + "Cen_ctrl"))->GetValue())->GetAsString())); + _sdf->RemoveChild(curr_ctrl_surface); + } + + this->AR = _sdf->Get("AR", this->AR).first; + this->mac = _sdf->Get("mac", this->mac).first; + this->eff = _sdf->Get("eff", this->eff).first; + this->rho = _sdf->Get("air_density", this->rho).first; + this->radialSymmetry = _sdf->Get("radial_symmetry", + this->radialSymmetry).first; + this->area = _sdf->Get("area", this->area).first; + + // blade forward (-drag) direction in link frame + this->forward = + _sdf->Get("forward", this->forward).first; + if(std::fabs(this->forward.Length()) >= 1e-9){ + this->forward.Normalize(); + } + + else + { + gzerr << "Forward vector length is zero. This is not valid.\n"; + this->validConfig = false; + return; + } + + // blade upward (+lift) direction in link frame + this->upward = _sdf->Get( + "upward", this->upward) .first; + this->upward.Normalize(); + + this->controlJointRadToCL = _sdf->Get( + "control_joint_rad_to_cl", this->controlJointRadToCL).first; + + if (_sdf->HasElement("link_name")) + { + sdf::ElementPtr elem = _sdf->GetElement("link_name"); + auto linkName = elem->Get(); + auto entities = + entitiesFromScopedName(linkName, _ecm, this->model.Entity()); + + if (entities.empty()) + { + gzerr << "Link with name[" << linkName << "] not found. " + << "The AdvancedLiftDrag will not generate forces.\n"; + this->validConfig = false; + return; + } + else if (entities.size() > 1) + { + gzwarn << "Multiple link entities with name[" << linkName << "] found. " + << "Using the first one.\n"; + } + + this->linkEntity = *entities.begin(); + if (!_ecm.EntityHasComponentType(this->linkEntity, + components::Link::typeId)) + { + this->linkEntity = kNullEntity; + gzerr << "Entity with name[" << linkName << "] is not a link.\n"; + this->validConfig = false; + return; + } + } + else + { + gzerr << "AdvancedLiftDrag system requires the 'link_name' parameter.\n"; + this->validConfig = false; + return; + } + + // If we reached here, we have a valid configuration + this->validConfig = true; +} + +////////////////////////////////////////////////// +AdvancedLiftDrag::AdvancedLiftDrag() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void AdvancedLiftDragPrivate::Update(EntityComponentManager &_ecm) +{ + GZ_PROFILE("AdvancedLiftDragPrivate::Update"); + // get linear velocity at cp in world frame + const auto worldLinVel = + _ecm.Component(this->linkEntity); + const auto worldAngVel = + _ecm.Component(this->linkEntity); + const auto worldPose = + _ecm.Component(this->linkEntity); + + // get wind as a component from the _ecm + components::WorldLinearVelocity *windLinearVel = nullptr; + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + windLinearVel = + _ecm.Component(windEntity); + } + + std::vector controlJointPosition_vec( + this->num_ctrl_surfaces); + // Generate a new vector that contains the current positions for all joints + for(int i = 0; i < this->num_ctrl_surfaces; i++){ + components::JointPosition *controlJointPosition = nullptr; + if(this->controlJoints[i] != kNullEntity){ + controlJointPosition = _ecm.Component( + this->controlJoints[i]); + controlJointPosition_vec[i] = controlJointPosition; + } + } + + if (!worldLinVel || !worldAngVel || !worldPose) + return; + + const auto &pose = worldPose->Data(); + const auto cpWorld = pose.Rot().RotateVector(this->cp); + auto air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld); + if (windLinearVel != nullptr){ + air_velocity = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld) - windLinearVel->Data(); + } + + // Define body frame: X forward, Z downward, Y out the right wing + gz::math::Vector3d body_x_axis = pose.Rot().RotateVector(this->forward); + gz::math::Vector3d body_z_axis = -1*(pose.Rot().RotateVector(this->upward)); + gz::math::Vector3d body_y_axis = body_z_axis.Cross(body_x_axis); + + // Get the in-plane velocity (remove spanwise velocity from the velocity + // vector air_velocity) + gz::math::Vector3d velInLDPlane = air_velocity - air_velocity.Dot( + body_y_axis)*body_y_axis; + + // Compute dynamic pressure + const double speedInLDPlane = velInLDPlane.Length(); + + // Define stability frame: X is in-plane velocity, Y is the same as body Y, + // and Z perpendicular to both + if(speedInLDPlane <= 1e-9){ + return; + } + gz::math::Vector3d stability_x_axis = velInLDPlane/speedInLDPlane; + gz::math::Vector3d stability_y_axis = body_y_axis; + gz::math::Vector3d stability_z_axis = stability_x_axis.Cross( + stability_y_axis); + + double span = std::sqrt(this->area*this->AR); + double epsilon = 1e-9; + if (fabs(this->mac - 0.0) <= epsilon){ + // Computing the mean aerodynamic chord involves integrating the square of + // the chord along the span. If this parameter has not been input, this + // plugin will approximate MAC as mean chord. This works for rectangular + // and trapezoidal wings, but for more complex wing shapes, doing the + // integral is preferred. + this->mac = this->area/span; + } + + // Get non-dimensional body rates. Gazebo uses ENU, so some have to be flipped + // gz::math::Vector3d body_rates = this->link->GetRelativeAngularVel(); + components::AngularVelocity *AngVel = nullptr; + if (this->linkEntity != kNullEntity) + { + AngVel = _ecm.Component(this->linkEntity); + } + if(AngVel == nullptr){ + gzerr << "Angular Velocity cannot be null.\n"; + this->validConfig = false; + return; + } + + double rr = AngVel->Data()[0]; // Roll rate + double pr = -1*AngVel->Data()[1]; // Pitch rate + double yr = -1*AngVel->Data()[2]; // Yaw rate + + // Compute angle of attack, alpha, using the stability and body axes + // Project stability x onto body x and z, then take arctan to find alpha + double stabx_proj_bodyx = stability_x_axis.Dot(body_x_axis); + double stabx_proj_bodyz = stability_x_axis.Dot(body_z_axis); + this->alpha = atan2(stabx_proj_bodyz, stabx_proj_bodyx); + + double sinAlpha = sin(this->alpha); + double cosAlpha = cos(this->alpha); + + // Compute sideslip angle, beta + double velSW = air_velocity.Dot(body_y_axis); + double velFW = air_velocity.Dot(body_x_axis); + this->beta = (atan2(velSW, velFW)); + + // Compute dynamic pressure + double dyn_pres = 0.5 * this->rho * speedInLDPlane * speedInLDPlane; + double half_rho_vel = 0.5 * this->rho * speedInLDPlane; + + // Compute CL at cp, check for stall + double CL{0.0}; + + // Use a sigmoid function to blend pre- and post-stall models + double sigma = (1+exp(-1*this->M*(this->alpha-this->alphaStall))+exp( + this->M*(this->alpha+this->alphaStall)))/((1+exp( + -1*this->M*(this->alpha-this->alphaStall)))*(1+exp( + this->M*(this->alpha+this->alphaStall)))); // blending function + + // The lift coefficient (as well as all the other coefficients) are + // defined with the coefficient build-up method and using a quasi-steady + // approach. The first thing that is calculated is the CL of the wing in + // steady conditions (normal CL-alpha curve). Secondly, the effect of the + // roll, pitch, and yaw is added through the AVL stability coefficients. + // Finally, the effect of the control surfaces is added. + + // The lift coefficient of the wing is defined as a combination of a linear + // function for the pre-stall regime and a combination of exponents a + // trigonometric functions for the post-stall regime. Both functions are + // blended in with the sigmoid function. + // CL_prestall = this->CL0 + this->CLa*ths->alpha + // CL_poststall = 2*(this->alpha/abs(this->alpha))*pow(sinAlpha,2.0)*cosAlpha + + CL = (1-sigma)*(this->CL0 + this->CLa*this->alpha) + sigma*( + 2*(this->alpha/abs(this->alpha))*sinAlpha*sinAlpha*cosAlpha); + + // Add sideslip effect, if any + CL = CL + this->CLb*this->beta; + + // Compute control surface effects + double CL_ctrl_tot = 0; + double CD_ctrl_tot = 0; + double CY_ctrl_tot = 0; + double Cell_ctrl_tot = 0; + double Cem_ctrl_tot = 0; + double Cen_ctrl_tot = 0; + + for(int i = 0; i < this->num_ctrl_surfaces; i++){ + double controlAngle = 0.0; + if (controlJointPosition_vec[i] && !controlJointPosition_vec[ + i]->Data().empty()) + { + components::JointPosition *tmp_controlJointPosition = + controlJointPosition_vec[i]; + controlAngle = tmp_controlJointPosition->Data()[0] * 180 / GZ_PI; + } + + // AVL's and Gazebo's direction of "positive" deflection may be different. + // Future users, keep an eye on this. + CL_ctrl_tot += controlAngle*this->CL_ctrl[i]* + this->ctrl_surface_direction[i]; + CD_ctrl_tot += controlAngle*this->CD_ctrl[i]* + this->ctrl_surface_direction[i]; + CY_ctrl_tot += controlAngle*this->CY_ctrl[i]* + this->ctrl_surface_direction[i]; + Cell_ctrl_tot += controlAngle*this->Cell_ctrl[i]* + this->ctrl_surface_direction[i]; + Cem_ctrl_tot += controlAngle*this->Cem_ctrl[i]* + this->ctrl_surface_direction[i]; + Cen_ctrl_tot += controlAngle*this->Cen_ctrl[i]* + this->ctrl_surface_direction[i]; + } + + // AVL outputs a "CZ_elev", but the Z axis is down. This plugin + // uses CL_elev, which is the negative of CZ_elev + CL = CL+CL_ctrl_tot; + + // Compute lift force at cp + gz::math::Vector3d lift = (CL * dyn_pres + (this->CLp * ( + rr*span/2) * half_rho_vel) + (this->CLq * (pr*this->mac/2) * + half_rho_vel) + (this->CLr * (yr*span/2) * half_rho_vel)) * + (this->area * (-1 * stability_z_axis)); + + // Compute CD at cp, check for stall + double CD{0.0}; + + // Add in quadratic model and a high-angle-of-attack (Newtonian) model + // The post stall model used below has two parts. The first part, the + // (estimated) flat plate drag, comes from the data in Ostowari and Naik, + // Post-Stall Wind Tunnel Data for NACA 44XX Series Airfoil Sections. + // https://www.nrel.gov/docs/legosti/old/2559.pdf + // The second part (0.5-0.5cos(2*alpha)) is fitted using data from + // Stringer et al, + // A new 360° airfoil model for predicting airfoil thrust potential in + // vertical-axis wind turbine designs. + // https://aip.scitation.org/doi/pdf/10.1063/1.5011207 + // I halved the drag numbers to make sure it would work with my + // flat plate drag model. + + + // To estimate the flat plate coefficient of drag, I fit a sigmoid function + // to the data in Ostowari and Naik. The form I used was: + // CD_FP = 2/(1+exp(k1+k2*AR)). + // The coefficients k1 and k2 might need some tuning. + // I chose a sigmoid because the CD would initially increase quickly + // with aspect ratio, but that rate of increase would slow down as AR + // goes to infinity. + + double CD_fp = 2 / (1 + exp(this->CD_fp_k1 + this->CD_fp_k2 * ( + std::max(this->AR, 1 / this->AR)))); + CD = (1 - sigma) * (this->CD0 + (CL*CL) / (GZ_PI * this->AR * + this->eff)) + sigma * abs( + CD_fp * (0.5 - 0.5 * cos(2 * this->alpha))); + + // Add in control surface terms + CD = CD + CD_ctrl_tot; + + // Place drag at cp + gz::math::Vector3d drag = (CD * dyn_pres + (this->CDp * (rr*span/2) * + half_rho_vel) + ( + this->CDq * (pr*this->mac/2) * half_rho_vel) + + (this->CDr * (yr*span/2) * half_rho_vel)) * (this->area * + (-1*stability_x_axis)); + + // Compute sideforce coefficient, CY + // Start with angle of attack, sideslip, and control terms + double CY = this->CYa * this->alpha + this->CYb * this->beta + CY_ctrl_tot; + + gz::math::Vector3d sideforce = (CY * dyn_pres + (this->CYp * ( + rr*span/2) * half_rho_vel) + (this->CYq * (pr*this->mac/2) * + half_rho_vel) + (this->CYr * (yr*span/2) * half_rho_vel)) * + (this->area * stability_y_axis); + + // The Cm is divided in three sections: alpha>stall angle, alpha<-stall + // angle-stall anglestall angle region the Cm is assumed to always be positive or + // zero, in the alpha<-stall angle the Cm is assumed to always be + // negative or zero. + + double Cem{0.0}; + + if (alpha > this->alphaStall) + { + Cem = this->Cem0 + (this->Cema * this->alphaStall + + this->CemaStall * (this->alpha - this->alphaStall)); + } + else if (alpha < -this->alphaStall) + { + Cem = this->Cem0 + (-this->Cema * this->alphaStall + + this->CemaStall * (this->alpha + this->alphaStall)); + } + else + { + Cem = this->Cem0 + this->Cema * this->alpha; + } + // Add sideslip effect, if any + Cem = this->Cemb * this->beta; + + Cem += Cem_ctrl_tot; + + gz::math::Vector3d pm = ((Cem * dyn_pres) + (this->Cemp * ( + rr*span/2) * half_rho_vel) + (this->Cemq * (pr*this->mac/2) * + half_rho_vel) + (this->Cemr * (yr*span/2) * half_rho_vel)) * + (this->area * this->mac * body_y_axis); + + // Compute roll moment coefficient, Cell + // Start with angle of attack, sideslip, and control terms + double Cell = this-> Cella * this->alpha + this->Cellb * + this-> beta + Cell_ctrl_tot; + gz::math::Vector3d rm = ((Cell * dyn_pres) + (this->Cellp * ( + rr*span/2) * half_rho_vel) + (this->Cellq * (pr*this->mac/2) * + half_rho_vel) + (this->Cellr * (yr*span/2) * half_rho_vel)) * + (this->area * span * body_x_axis); + + // Compute yaw moment coefficient, Cen + // Start with angle of attack, sideslip, and control terms + double Cen = this->Cena * this->alpha + this->Cenb * this->beta + + Cen_ctrl_tot; + gz::math::Vector3d ym = ((Cen * dyn_pres) + (this->Cenp * ( + rr*span/2) * half_rho_vel) + (this->Cenq * (pr*this->mac/2) * + half_rho_vel) + (this->Cenr * (yr*span/2) * half_rho_vel)) * + (this->area * span * body_z_axis); + + // Compute moment (torque) + gz::math::Vector3d moment = pm+rm+ym; + + // Compute force about cg in inertial frame + gz::math::Vector3d force = lift + drag + sideforce; + gz::math::Vector3d torque = moment; + + // Correct for nan or inf + force.Correct(); + this->cp.Correct(); + torque.Correct(); + + // positions + const auto totalTorque = torque + cpWorld.Cross(force); + Link link(this->linkEntity); + link.AddWorldWrench(_ecm, force, totalTorque); +} + +////////////////////////////////////////////////// +void AdvancedLiftDrag::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, EventManager &) +{ + this->dataPtr->model = Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "Advanced LiftDrag system should be attached to a model entity." + << "Failed to initialize." << std::endl; + return; + } + this->dataPtr->sdfConfig = _sdf->Clone(); +} + +////////////////////////////////////////////////// +void AdvancedLiftDrag::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("AdvancedLiftDrag::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + if (!this->dataPtr->initialized) + { + // We call Load here instead of Configure because we can't be guaranteed + // that all entities have been created when Configure is called + this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); + this->dataPtr->initialized = true; + + if (this->dataPtr->validConfig) + { + Link link(this->dataPtr->linkEntity); + link.EnableVelocityChecks(_ecm, true); + + for(int i = 0; i < this->dataPtr->num_ctrl_surfaces; i++){ + + if ((this->dataPtr->controlJoints[i]!= kNullEntity) && + !_ecm.Component(this->dataPtr-> + controlJoints[i])) + { + _ecm.CreateComponent(this->dataPtr->controlJoints[i], + components::JointPosition()); + } + } + } + } + + if (_info.paused) + return; + + // This is not an "else" because "initialized" can be set in the if block + // above + if (this->dataPtr->initialized && this->dataPtr->validConfig) + { + + this->dataPtr->Update(_ecm); + } +} + +GZ_ADD_PLUGIN(AdvancedLiftDrag, + System, + AdvancedLiftDrag::ISystemConfigure, + AdvancedLiftDrag::ISystemPreUpdate) + +GZ_ADD_PLUGIN_ALIAS(AdvancedLiftDrag, + "gz::sim::systems::AdvancedLiftDrag") diff --git a/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh b/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh new file mode 100644 index 0000000000..31b420d745 --- /dev/null +++ b/src/systems/advanced_lift_drag/AdvancedLiftDrag.hh @@ -0,0 +1,154 @@ +/* + * 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. + * + */ + +#ifndef GZ_SIM_SYSTEMS_ADVANCED_LIFT_DRAG_HH_ +#define GZ_SIM_SYSTEMS_ADVANCED_LIFT_DRAG_HH_ + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class AdvancedLiftDragPrivate; + + /// \brief The LiftDrag system computes lift and drag forces enabling + /// simulation of aerodynamic robots. + /// + /// + /// A tool can be found at the link below that automatically generates + /// the Advanced Lift Drag plugin for you. All that is needed is to + /// provide some physical parameters of the model. The README contains + /// all necessary setup and usage steps. + /// + /// https://github.com/PX4/PX4-Autopilot/blob/main/Tools/simulation/ + /// gz/tools/avl_automation/ + /// + /// Note: Wind calculations can be enabled by setting the wind parameter + /// in the world file. + /// + /// ## System Parameters + /// + /// - ``: Name of the link affected by the group of lift/drag + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. + /// - ``: Density of the fluid this model is suspended in. + /// - ``: Surface area of the link. + /// - ``: Lift Coefficient at zero angle of attack + /// - ``: Pitching moment coefficient at zero angle of attack + /// - ``: Drag Coefficient at zero angle of attack + /// - ``: dCL/da (slope of CL-alpha curve) Slope of the first + /// portion of the alpha-lift coefficient curve. + /// - ``: dCy/da (sideforce slope wrt alpha) + /// - ``: dCl/da (roll moment slope wrt alpha) + /// - ``: dCm/da (pitching moment slope wrt alpha - before stall) + /// - ``: dCn/da (yaw moment slope wrt alpha) + /// - ``: dCL/dbeta (lift coefficient slope wrt beta) + /// - ``: dCY/dbeta (side force slope wrt beta) + /// - ``: dCl/dbeta (roll moment slope wrt beta) + /// - ``: dCm/dbeta (pitching moment slope wrt beta) + /// - ``: dCn/dbeta (yaw moment slope wrt beta) + /// - ``: angle of attack when airfoil stalls + /// - ``: Slope of the Cm-alpha curve after stall + /// - ``: 3-vector replacing the center of pressure in the original + /// LiftDragPlugin. + /// - ``: dCD/dp (drag coefficient slope wrt roll rate) + /// - ``: dCY/dp (sideforce slope wrt roll rate) + /// - ``: dCL/dp (lift coefficient slope wrt roll rate) + /// - ``: dCl/dp (roll moment slope wrt roll rate) + /// - ``: dCn/dp (yaw moment slope wrt roll rate) + /// - ``: dCD/dq (drag coefficient slope wrt pitching rate) + /// - ``: dCY/dq (side force slope wrt pitching rate) + /// - ``: dCL/dq (lift coefficient slope wrt pitching rate) + /// - ``: dCl/dq (roll moment slope wrt pitching rate) + /// - ``: dCm/dq (pitching moment slope wrt pitching rate) + /// - ``: dCn/dq (yaw moment slope wrt pitching rate) + /// - ``: dCD/dr (drag coefficient slope wrt yaw rate) + /// - ``: dCY/dr (side force slope wrt yaw rate) + /// - ``: dCL/dr (lift coefficient slope wrt yaw rate) + /// - ``: dCl/dr (roll moment slope wrt yaw rate) + /// - ``: dCm/dr (pitching moment slope wrt yaw rate) + /// - ``: dCn/dr (yaw moment slope wrt yaw rate) + /// - ``: Number of control surfaces + /// - ``: Vector that points to the joints that connect to the + /// control surface + /// - ``: Vectors of control surface deflections + /// - ``: Vector of the effect of the deflection on the coefficient + /// of drag + /// - ``: Vector of the effect of the deflection on the coefficient + /// of side force + /// - ``: Vector of the effect of the deflection on the coefficient + /// of lift + /// - ``: Vector of the effect of the deflection on the coefficient + /// of roll moment + /// - ``: Vector of the effect of the deflection on the coefficient + /// of pitching moment + /// - ``: Vector of the effect of the deflection on the coefficient + /// of yaw moment + /// - ``: Aspect ratio + /// - ``: The mean-aerodynamic chord + /// - ``: Wing efficiency (This is the Oswald efficiency factor for a + /// 3D wing) + /// - ``: The ratio of the coefficient of drag and alpha slope before + /// stall. + /// - ``: 3-vector representing the forward direction of motion in + /// the link frame. + /// - ``: 3-vector representing the direction of lift or drag. + /// - ``: Angle of attack at stall point; peak angle of attack. + /// - ``: The angle of attack + /// - ``: The sideslip angle + /// - ``: The sigmoid blending parameter + /// - ``: The first of the flat plate drag model coefficients + /// - ``: The second of the flat plate drag model coefficients + /// - ``: Copy of the sdf configuration used for this plugin + + class AdvancedLiftDrag + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: AdvancedLiftDrag(); + + /// \brief Destructor + public: ~AdvancedLiftDrag() override = default; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/advanced_lift_drag/CMakeLists.txt b/src/systems/advanced_lift_drag/CMakeLists.txt new file mode 100644 index 0000000000..271f9d06a1 --- /dev/null +++ b/src/systems/advanced_lift_drag/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(advanced-lift-drag + SOURCES + AdvancedLiftDrag.cc +) diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.hh b/src/systems/battery_plugin/LinearBatteryPlugin.hh index f82afa7515..2f142d37e5 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.hh +++ b/src/systems/battery_plugin/LinearBatteryPlugin.hh @@ -39,12 +39,13 @@ namespace systems /// \brief A plugin for simulating battery usage /// - /// This system processes the following sdf parameters: + /// ## System parameters + /// /// - `` name of the battery (required) /// - `` Initial voltage of the battery (required) /// - `` Voltage at full charge /// - `` Amount of voltage decrease when no - /// charge + /// charge /// - `` Initial charge of the battery (Ah) /// - `` Total charge that the battery can hold (Ah) /// - `` Internal resistance (Ohm) @@ -52,22 +53,22 @@ namespace systems /// - `` power load on battery (required) (Watts) /// - `` If true, the battery can be recharged /// - `` If true, the start/stop signals for recharging the - /// battery will also be available via topics. The - /// regular Gazebo services will still be available. + /// battery will also be available via topics. The + /// regular Gazebo services will still be available. /// - `` Hours taken to fully charge the battery. - /// (Required if `` is set to true) + /// (Required if `` is set to true) /// - `` True to change the battery behavior to fix some issues - /// described in https://github.com/gazebosim/gz-sim/issues/225. + /// described in https://github.com/gazebosim/gz-sim/issues/225. /// - `` Whether to start draining the battery right away. - /// False by default. + /// False by default. /// - `` A topic that is used to start battery - /// discharge. Any message on the specified topic will cause the battery to - /// start draining. This element can be specified multiple times if - /// multiple topics should be monitored. Note that this mechanism will - /// start the battery draining, and once started will keep drainig. + /// discharge. Any message on the specified topic will cause the battery to + /// start draining. This element can be specified multiple times if + /// multiple topics should be monitored. Note that this mechanism will + /// start the battery draining, and once started will keep drainig. /// - `` A topic that is used to stop battery - /// discharge. Any message on the specified topic will cause the battery to - /// stop draining. + /// discharge. Any message on the specified topic will cause the battery to + /// stop draining. class LinearBatteryPlugin : public System, diff --git a/src/systems/breadcrumbs/Breadcrumbs.hh b/src/systems/breadcrumbs/Breadcrumbs.hh index 5e5b3f64e4..0e09180263 100644 --- a/src/systems/breadcrumbs/Breadcrumbs.hh +++ b/src/systems/breadcrumbs/Breadcrumbs.hh @@ -56,7 +56,7 @@ namespace systems /// model using the `` tag. /// See the example in examples/worlds/breadcrumbs.sdf. /// - /// System Paramters + /// ## System Parameters /// /// - ``: Custom topic to be used to deploy breadcrumbs. If topic is /// not set, the default topic with the following pattern would be used @@ -82,7 +82,7 @@ namespace systems /// be deployed. Defaults to false. /// - ``: This is the model used as a template for deploying /// breadcrumbs. - /// ``: If true, then topic statistics are enabled on + /// - ``: If true, then topic statistics are enabled on /// `` and error messages will be generated when messages are /// dropped. Default to false. class Breadcrumbs diff --git a/src/systems/buoyancy/Buoyancy.hh b/src/systems/buoyancy/Buoyancy.hh index 5eae0f0864..bf89e01efb 100644 --- a/src/systems/buoyancy/Buoyancy.hh +++ b/src/systems/buoyancy/Buoyancy.hh @@ -67,7 +67,8 @@ namespace systems /// /// ## Examples /// - /// ### `uniform_fluid_density` world. + /// ### uniform_fluid_density world + /// /// The `buoyancy.sdf` SDF file contains three submarines. The first /// submarine is neutrally buoyant, the second sinks, and the third /// floats. To run: @@ -76,7 +77,7 @@ namespace systems /// gz sim -v 4 buoyancy.sdf /// ``` /// - /// ### `graded_buoyancy` world + /// ### graded_buoyancy world /// /// Often when simulating a maritime environment one may need to simulate both /// surface and underwater vessels. This means the buoyancy plugin needs to diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh index ab0ad73324..d7a92b2996 100644 --- a/src/systems/buoyancy_engine/BuoyancyEngine.hh +++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh @@ -38,32 +38,35 @@ namespace systems /// `/model/{namespace}/buoyancy_engine` topic for the volume of the bladder /// in *cubicmeters*. /// - /// ## Parameters - /// - The link which the plugin is attached to [required, string] - /// - The namespace for the topic. If empty the plugin will listen - /// on `buoyancy_engine` otherwise it listens on + /// ## System Parameters + /// - ``: The link which the plugin is attached to [required, + /// string] + /// - ``: The namespace for the topic. If empty the plugin will + /// listen on `buoyancy_engine` otherwise it listens on /// `/model/{namespace}/buoyancy_engine` [optional, string] - /// - Minimum volume of the engine [optional, float, + /// - ``: Minimum volume of the engine [optional, float, /// default=0.00003m^3] - /// - At this volume the engine has neutral buoyancy. Used to - /// estimate the weight of the engine [optional, float, default=0.0003m^3] - /// - The volume which the engine starts at [optional, float, + /// - ``: At this volume the engine has neutral buoyancy. Used + /// to estimate the weight of the engine [optional, float, /// default=0.0003m^3] - /// - Maximum volume of the engine [optional, float, + /// - ``: The volume which the engine starts at [optional, + /// float, default=0.0003m^3] + /// - ``: Maximum volume of the engine [optional, float, /// default=0.00099m^3] - /// - Maximum inflation rate for bladder [optional, + /// - ``: Maximum inflation rate for bladder [optional, /// float, default=0.000003m^3/s] - /// - The fluid density of the liquid its suspended in kgm^-3. - /// [optional, float, default=1000kgm^-3] - /// - The Z height in metres at which the surface of the water is. - /// If not defined then there is no surface [optional, float] + /// - ``: The fluid density of the liquid its suspended in + /// kgm^-3. [optional, float, default=1000kgm^-3] + /// - ``: The Z height in metres at which the surface of the water + /// is. If not defined then there is no surface [optional, float] /// /// ## Topics - /// * Subscribes to a gz::msgs::Double on `buoyancy_engine` or - /// `/model/{namespace}/buoyancy_engine`. This is the set point for the - /// engine. - /// * Publishes a gz::msgs::Double on `buoyancy_engine` or - /// `/model/{namespace}/buoyancy_engine/current_volume` on the current volume + /// - Subscribes to a gz::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine`. This is the set point for the + /// engine. + /// - Publishes a gz::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine/current_volume` on the current + /// volume /// /// ## Examples /// To get started run: @@ -71,19 +74,22 @@ namespace systems /// gz sim buoyancy_engine.sdf /// ``` /// Enter the following in a separate terminal: - /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double - /// -p "data: 0.003" - /// ``` - /// To see the box float up. - /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double - /// -p "data: 0.001" - /// ``` - /// To see the box go down. + /** ``` + gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ + -p "data: 0.003" + ``` + **/ + /// to see the box float up. + /** ``` + gz topic -t /model/buoyant_box/buoyancy_engine/ -m gz.msgs.Double \ + -p "data: 0.001" + ``` + **/ + /// to see the box go down. + /// /// To see the current volume enter: /// ``` - /// gz topic -t /model/buoyant_box/buoyancy_engine/current_volume -e + /// gz topic -t /model/buoyant_box/buoyancy_engine/current_volume -e /// ``` class BuoyancyEnginePlugin: public gz::sim::System, diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.hh b/src/systems/camera_video_recorder/CameraVideoRecorder.hh index f19369cd13..2d3115e2a0 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.hh +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.hh @@ -34,23 +34,25 @@ namespace systems /** \class CameraVideoRecorder CameraVideoRecorder.hh \ * gz/sim/systems/CameraVideoRecorder.hh **/ + /// /// \brief Record video from a camera sensor - /// The system takes in the following parameter: - /// Name of topic for the video recorder service. If this is - /// not specified, the topic defaults to: - /// /world//link// - /// sensor//record_video /// - /// True/false value that specifies if the video should - /// be recorded using simulation time or real time. The - /// default is false, which indicates the use of real - /// time. + /// ## System Parameters + /// + /// - ``: Name of topic for the video recorder service. If this is + /// not specified, the topic defaults to: + /// /world//link// + /// sensor//record_video + /// + /// - ``: True/false value that specifies if the video should + /// be recorded using simulation time or real time. The default is false, + /// which indicates the use of real time. /// - /// Video recorder frames per second. The default value is 25, and - /// the support type is unsigned int. + /// - ``: Video recorder frames per second. The default value is 25, and + /// the support type is unsigned int. /// - /// Video recorder bitrate (bps). The default value is - /// 2070000 bps, and the supported type is unsigned int. + /// - ``: Video recorder bitrate (bps). The default value is + /// 2070000 bps, and the supported type is unsigned int. class CameraVideoRecorder final: public System, public ISystemConfigure, diff --git a/src/systems/comms_endpoint/CommsEndpoint.hh b/src/systems/comms_endpoint/CommsEndpoint.hh index 30dacdb560..4e43b7bbba 100644 --- a/src/systems/comms_endpoint/CommsEndpoint.hh +++ b/src/systems/comms_endpoint/CommsEndpoint.hh @@ -36,22 +36,23 @@ namespace systems /// running. The system will bind this address in the broker automatically /// for you and unbind it when the model is destroyed. /// - /// The endpoint can be configured with the following SDF parameters: + /// ## System Parameters /// - /// * Required parameters: - ///
An identifier used to receive messages (string). - /// The topic name where you want to receive the messages targeted to - /// this address. + /// Required parameters: + /// - `
`: An identifier used to receive messages (string). + /// - ``: The topic name where you want to receive the messages + /// targeted to this address. /// - /// * Optional parameters: - /// Element used to capture where are the broker services. - /// This block can contain any of the next optional parameters: - /// : Service name used to bind an address. - /// The default value is "/broker/bind" - /// : Service name used to unbind from an address. - /// The default value is "/broker/unbind" + /// Optional parameters: + /// - ``: Element used to capture where are the broker services. + /// This block can contain any of the next optional parameters: + /// - ``: Service name used to bind an address. + /// The default value is "/broker/bind" + /// - ``: Service name used to unbind from an address. + /// The default value is "/broker/unbind" /// - /// Here's an example: + /// ## Example + /// ``` /// @@ -62,6 +63,7 @@ namespace systems /// /broker/unbind /// /// + /// ``` class CommsEndpoint : public System, public ISystemConfigure, diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index e6cb491285..c207b2d2ac 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -40,7 +40,7 @@ namespace systems /// model can be re-attached during simulation via a topic. The status of the /// detached state can be monitored via a topic as well. /// - /// Parameters: + /// ## System Parameters /// /// - ``: Name of the link in the parent model to be used in /// creating a fixed joint with a link in the child model. diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index 150621bb7c..0d16498e92 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -35,99 +35,99 @@ namespace systems /// \brief Differential drive controller which can be attached to a model /// with any number of left and right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of a joint that controls a left wheel. This + /// - ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a right wheel. This + /// - ``: Name of a joint that controls a right wheel. This /// element can appear multiple times, and must appear at least once. /// - /// ``: Distance between wheels, in meters. This element + /// - ``: Distance between wheels, in meters. This element /// is optional, although it is recommended to be included with an /// appropriate value. The default value is 1.0m. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the /// default value is `/model/{name_of_model}/cmd_vel`. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element is optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, and the + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry trasnform in both the `` - /// `gz.msgs.Pose_V` message and the `` - /// `gz.msgs.Odometry` message. This element if optional, + /// gz.msgs.Pose_V message and the `` + /// gz.msgs.Odometry message. This element if optional, /// and the default value is `{name_of_model}/{name_of_link}`. /// - /// ``: Sets both the minimum linear and minimum angular + /// - ``: Sets both the minimum linear and minimum angular /// velocity. /// - /// ``: Sets both the maximum linear and maximum angular + /// - ``: Sets both the maximum linear and maximum angular /// velocity. /// - /// ``: Sets both the minimum linear and minimum angular + /// - ``: Sets both the minimum linear and minimum angular /// acceleration. /// - /// ``: Sets both the maximum linear and maximum angular + /// - ``: Sets both the maximum linear and maximum angular /// acceleration. /// - /// ``: Sets both the minimum linear and minimum angular jerk. + /// - ``: Sets both the minimum linear and minimum angular jerk. /// - /// ``: Sets both the maximum linear and maximum angular jerk. + /// - ``: Sets both the maximum linear and maximum angular jerk. /// - /// ``: Sets the minimum linear velocity. Overrides + /// - ``: Sets the minimum linear velocity. Overrides /// `` if set. /// - /// ``: Sets the maximum linear velocity. Overrides + /// - ``: Sets the maximum linear velocity. Overrides /// `` if set. /// - /// ``: Sets the minimum angular velocity. Overrides + /// - ``: Sets the minimum angular velocity. Overrides /// `` if set. /// - /// ``: Sets the maximum angular velocity. Overrides + /// - ``: Sets the maximum angular velocity. Overrides /// `` if set. /// - /// ``: Sets the minimum linear acceleration. + /// - ``: Sets the minimum linear acceleration. /// Overrides `` if set. /// - /// ``: Sets the maximum linear acceleration. + /// - ``: Sets the maximum linear acceleration. /// Overrides `` if set. /// - /// ``: Sets the minimum angular acceleration. + /// - ``: Sets the minimum angular acceleration. /// Overrides `` if set. /// - /// ``: Sets the maximum angular acceleration. + /// - ``: Sets the maximum angular acceleration. /// Overrides `` if set. /// - /// ``: Sets the minimum linear jerk. Overrides `` - /// if set. + /// - ``: Sets the minimum linear jerk. Overrides + /// `` if set. /// - /// ``: Sets the maximum linear jerk. Overrides `` - /// if set. + /// - ``: Sets the maximum linear jerk. Overrides + /// `` if set. /// - /// ``: Sets the minimum angular jerk. Overrides + /// - ``: Sets the minimum angular jerk. Overrides /// `` if set. /// - /// ``: Sets the maximum angular jerk. Overrides + /// - ``: Sets the maximum angular jerk. Overrides /// `` if set. class DiffDrive : public System, diff --git a/src/systems/elevator/Elevator.hh b/src/systems/elevator/Elevator.hh index 0fa6f0ea52..dc188795ee 100644 --- a/src/systems/elevator/Elevator.hh +++ b/src/systems/elevator/Elevator.hh @@ -69,37 +69,37 @@ class ElevatorPrivate; /// /// ## System Parameters /// -/// ``: System update rate. This element is optional and the +/// - ``: System update rate. This element is optional and the /// default value is 10Hz. A value of zero gets translated to the simulation /// rate (no throttling for the system). /// -/// ``: Prefix in the names of the links that function as +/// - ``: Prefix in the names of the links that function as /// a reference for each floor level. When the elevator is requested to move /// to a given floor level, the cabin is commanded to move to the height of /// the corresponding floor link. The names of the links will be expected to /// be `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This /// element is optional and the default value is `floor_`. /// -/// ``: Prefix in the names of the joints that control the +/// - ``: Prefix in the names of the joints that control the /// doors of the elevator. The names of the joints will be expected to be /// `{prerix}i`, where \f$i=[0,N)\f$ and N is the number of floors. This /// element is optional and the default value is `door_`. /// -/// ``: Name of the joint that controls the position of the +/// - ``: Name of the joint that controls the position of the /// cabin. This element is optional and the default value is `lift`. /// -/// ``: Topic to which this system will subscribe in order to +/// - ``: Topic to which this system will subscribe in order to /// receive command messages. This element is optional and the default value /// is `/model/{model_name}/cmd`. /// -/// ``: Topic on which this system will publish state (current +/// - ``: Topic on which this system will publish state (current /// floor) messages. This element is optional and the default value is /// `/model/{model_name}/state`. /// -/// ``: State publication rate. This rate is bounded by +/// - ``: State publication rate. This rate is bounded by /// ``. This element is optional and the default value is 5Hz. /// -/// ``: Time to wait with a door open before the door +/// - ``: Time to wait with a door open before the door /// closes. This element is optional and the default value is 5 sec. class GZ_SIM_VISIBLE Elevator : public System, public ISystemConfigure, diff --git a/src/systems/environment_preload/CMakeLists.txt b/src/systems/environment_preload/CMakeLists.txt index 178ec5ec32..ed6a4b8efe 100644 --- a/src/systems/environment_preload/CMakeLists.txt +++ b/src/systems/environment_preload/CMakeLists.txt @@ -1,6 +1,7 @@ gz_add_system(environment-preload SOURCES EnvironmentPreload.cc + VisualizationTool.cc PUBLIC_LINK_LIBS gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-common${GZ_COMMON_VER}::io diff --git a/src/systems/environment_preload/EnvironmentPreload.cc b/src/systems/environment_preload/EnvironmentPreload.cc index 5c678aa2db..031dc21ee9 100644 --- a/src/systems/environment_preload/EnvironmentPreload.cc +++ b/src/systems/environment_preload/EnvironmentPreload.cc @@ -15,8 +15,10 @@ * */ #include "EnvironmentPreload.hh" +#include "VisualizationTool.hh" #include +#include #include #include #include @@ -27,6 +29,11 @@ #include +#include + +#include +#include + #include "gz/sim/components/Environment.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Util.hh" @@ -35,72 +42,114 @@ using namespace gz; using namespace sim; using namespace systems; +using Units = msgs::DataLoadPathOptions_DataAngularUnits; ////////////////////////////////////////////////// class gz::sim::systems::EnvironmentPreloadPrivate { + /// \brief Is the file loaded public: bool loaded{false}; + /// \brief SDF Description public: std::shared_ptr sdf; -}; -////////////////////////////////////////////////// -EnvironmentPreload::EnvironmentPreload() - : System(), dataPtr(new EnvironmentPreloadPrivate) -{ -} + /// \brief GzTransport node + public: transport::Node node; -////////////////////////////////////////////////// -EnvironmentPreload::~EnvironmentPreload() = default; + /// \brief Data descriptions + public: msgs::DataLoadPathOptions dataDescription; -////////////////////////////////////////////////// -void EnvironmentPreload::Configure( - const Entity &/*_entity*/, - const std::shared_ptr &_sdf, - EntityComponentManager &/*_ecm*/, - EventManager &/*_eventMgr*/) -{ - this->dataPtr->sdf = _sdf; -} + /// \brief mutex to protect the samples and data description + public: std::mutex mtx; -////////////////////////////////////////////////// -void EnvironmentPreload::PreUpdate( - const gz::sim::UpdateInfo &, - gz::sim::EntityComponentManager &_ecm) -{ - if (!std::exchange(this->dataPtr->loaded, true)) + /// \brief Do we need to reload the system. + public: std::atomic needsReload{false}; + + /// \brief Visualization Helper + public: std::unique_ptr visualizationPtr; + + /// \brief Are visualizations enabled + public: bool visualize{false}; + + /// \brief Sample resolutions + public: math::Vector3d samples; + + /// \brief Is the file loaded + public: bool fileLoaded{false}; + + /// \brief File loading error logger + public: bool logFileLoadError{true}; + + /// \brief Reference to data + public: std::shared_ptr envData; + + ////////////////////////////////////////////////// + public: EnvironmentPreloadPrivate() : + visualizationPtr(new EnvironmentVisualizationTool) {} + + ////////////////////////////////////////////////// + public: void OnLoadCommand(const msgs::DataLoadPathOptions &_msg) + { + std::lock_guard lock(this->mtx); + this->dataDescription = _msg; + this->needsReload = true; + this->logFileLoadError = true; + this->visualizationPtr->FileReloaded(); + gzdbg << "Loading file " << _msg.path() << "\n"; + } + + ////////////////////////////////////////////////// + public: void OnVisualResChanged(const msgs::Vector3d &_resChanged) + { + std::lock_guard lock(this->mtx); + if (!this->fileLoaded) + { + // Only visualize if a file exists + return; + } + auto converted = msgs::Convert(_resChanged); + if (this->samples == converted) + { + // If the sample has not changed return. + // This is because resampling is expensive. + return; + } + this->samples = converted; + this->visualize = true; + this->visualizationPtr->resample = true; + } + + ////////////////////////////////////////////////// + public: void ReadSdf(EntityComponentManager &_ecm) { - if (!this->dataPtr->sdf->HasElement("data")) + if (!this->sdf->HasElement("data")) { - gzerr << "No environmental data file was specified"; + gzerr << "No environmental data file was specified" << std::endl; return; } + std::lock_guard lock(mtx); std::string dataPath = - this->dataPtr->sdf->Get("data"); + this->sdf->Get("data"); + if (common::isRelativePath(dataPath)) { - auto * component = + auto *component = _ecm.Component(worldEntity(_ecm)); const std::string rootPath = common::parentPath(component->Data().Element()->FilePath()); dataPath = common::joinPaths(rootPath, dataPath); } + this->dataDescription.set_path(dataPath); - std::ifstream dataFile(dataPath); - if (!dataFile.is_open()) - { - gzerr << "No environmental data file was found at " << dataPath; - return; - } - - components::EnvironmentalData::ReferenceUnits unit{ - components::EnvironmentalData::ReferenceUnits::RADIANS}; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_RADIANS); std::string timeColumnName{"t"}; bool ignoreTime = false; std::array spatialColumnNames{"x", "y", "z"}; - auto spatialReference = math::SphericalCoordinates::GLOBAL; sdf::ElementConstPtr elem = - this->dataPtr->sdf->FindElement("dimensions"); + this->sdf->FindElement("dimensions"); + msgs::SphericalCoordinatesType spatialReference = + msgs::SphericalCoordinatesType::GLOBAL; if (elem) { if (elem->HasElement("ignore_time")) @@ -120,17 +169,18 @@ void EnvironmentPreload::PreUpdate( elem->Get("reference"); if (referenceName == "global") { - spatialReference = math::SphericalCoordinates::GLOBAL; + spatialReference = msgs::SphericalCoordinatesType::GLOBAL; } else if (referenceName == "spherical") { - spatialReference = math::SphericalCoordinates::SPHERICAL; + spatialReference = msgs::SphericalCoordinatesType::SPHERICAL; if (elem->HasAttribute("units")) { std::string unitName = elem->Get("units"); if (unitName == "degrees") { - unit = components::EnvironmentalData::ReferenceUnits::DEGREES; + this->dataDescription.set_units( + Units::DataLoadPathOptions_DataAngularUnits_DEGREES); } else if (unitName != "radians") { @@ -140,7 +190,7 @@ void EnvironmentPreload::PreUpdate( } else if (referenceName == "ecef") { - spatialReference = math::SphericalCoordinates::ECEF; + spatialReference = msgs::SphericalCoordinatesType::ECEF; } else { @@ -160,26 +210,143 @@ void EnvironmentPreload::PreUpdate( } } + this->dataDescription.set_static_time(ignoreTime); + this->dataDescription.set_coordinate_type(spatialReference); + this->dataDescription.set_time(timeColumnName); + this->dataDescription.set_x(spatialColumnNames[0]); + this->dataDescription.set_y(spatialColumnNames[1]); + this->dataDescription.set_z(spatialColumnNames[2]); + + this->needsReload = true; + } + + ////////////////////////////////////////////////// + public: components::EnvironmentalData::ReferenceUnits ConvertUnits( + const Units &_unit) + { + switch (_unit) + { + case Units::DataLoadPathOptions_DataAngularUnits_DEGREES: + return components::EnvironmentalData::ReferenceUnits::DEGREES; + case Units::DataLoadPathOptions_DataAngularUnits_RADIANS: + return components::EnvironmentalData::ReferenceUnits::RADIANS; + default: + gzerr << "Invalid unit conversion. Defaulting to radians." << std::endl; + return components::EnvironmentalData::ReferenceUnits::RADIANS; + } + } + + ////////////////////////////////////////////////// + public: void LoadEnvironment(EntityComponentManager &_ecm) + { try { - gzmsg << "Loading Environment Data\n"; + std::lock_guard lock(this->mtx); + std::array spatialColumnNames{ + this->dataDescription.x(), + this->dataDescription.y(), + this->dataDescription.z()}; + + math::SphericalCoordinates::CoordinateType spatialReference = + msgs::Convert(this->dataDescription.coordinate_type()); + auto units = this->ConvertUnits(this->dataDescription.units()); + + std::ifstream dataFile(this->dataDescription.path()); + if (!dataFile.is_open()) + { + if (this->logFileLoadError) + { + gzerr << "No environmental data file was found at " << + this->dataDescription.path() << std::endl; + logFileLoadError = false; + } + return; + } + + gzmsg << "Loading Environment Data " << this->dataDescription.path() << + std::endl; + using ComponentDataT = components::EnvironmentalData; auto data = ComponentDataT::MakeShared( common::IO::ReadFrom( common::CSVIStreamIterator(dataFile), common::CSVIStreamIterator(), - timeColumnName, spatialColumnNames), - spatialReference, unit, ignoreTime); - + this->dataDescription.time(), spatialColumnNames), + spatialReference, units, this->dataDescription.static_time()); + this->envData = data; using ComponentT = components::Environment; auto component = ComponentT{std::move(data)}; _ecm.CreateComponent(worldEntity(_ecm), std::move(component)); + this->visualizationPtr->resample = true; + this->fileLoaded = true; } catch (const std::invalid_argument &exc) { - gzerr << "Failed to load environment data" << std::endl - << exc.what() << std::endl; + if (this->logFileLoadError) + { + gzerr << "Failed to load environment data" << std::endl + << exc.what() << std::endl; + this->logFileLoadError = false; + } } + + this->needsReload = false; + } +}; + +////////////////////////////////////////////////// +EnvironmentPreload::EnvironmentPreload() + : System(), dataPtr(new EnvironmentPreloadPrivate) +{ +} + +////////////////////////////////////////////////// +EnvironmentPreload::~EnvironmentPreload() = default; + +////////////////////////////////////////////////// +void EnvironmentPreload::Configure( + const Entity &/*_entity*/, + const std::shared_ptr &_sdf, + EntityComponentManager &/*_ecm*/, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->sdf = _sdf; +} + +////////////////////////////////////////////////// +void EnvironmentPreload::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + if (!std::exchange(this->dataPtr->loaded, true)) + { + auto world = worldEntity(_ecm); + + // See https://github.com/gazebosim/gz-sim/issues/1786 + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment"), + &EnvironmentPreloadPrivate::OnLoadCommand, this->dataPtr.get()); + this->dataPtr->node.Subscribe( + transport::TopicUtils::AsValidTopic( + scopedName(world, _ecm) + "/environment/visualize/res"), + &EnvironmentPreloadPrivate::OnVisualResChanged, this->dataPtr.get()); + + this->dataPtr->visualizationPtr->resample = true; + this->dataPtr->ReadSdf(_ecm); + } + + if (this->dataPtr->needsReload) + { + this->dataPtr->LoadEnvironment(_ecm); + } + + if (this->dataPtr->visualize) + { + std::lock_guard lock(this->dataPtr->mtx); + auto samples = this->dataPtr->samples; + this->dataPtr->visualizationPtr->Step(_info, _ecm, this->dataPtr->envData, + samples.X(), samples.Y(), samples.Z()); } } diff --git a/src/systems/environment_preload/EnvironmentPreload.hh b/src/systems/environment_preload/EnvironmentPreload.hh index 16a9793cc0..9b462281c1 100644 --- a/src/systems/environment_preload/EnvironmentPreload.hh +++ b/src/systems/environment_preload/EnvironmentPreload.hh @@ -32,8 +32,9 @@ namespace systems { class EnvironmentPreloadPrivate; - /// \class EnvironmentPreload EnvironmentPreload.hh - /// gz/sim/systems/EnvironmentPreload.hh + /** \class EnvironmentPreload EnvironmentPreload.hh \ + * gz/sim/systems/EnvironmentPreload.hh + **/ /// \brief A plugin to preload an Environment component /// into the ECM upon simulation start-up. class EnvironmentPreload : diff --git a/src/systems/environment_preload/VisualizationTool.cc b/src/systems/environment_preload/VisualizationTool.cc new file mode 100644 index 0000000000..75b37b6557 --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.cc @@ -0,0 +1,232 @@ +/* + * 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 "VisualizationTool.hh" + +///////////////////////////////////////////////// +EnvironmentVisualizationTool::EnvironmentVisualizationTool() +{ + this->pcPub = + this->node.Advertise("/point_cloud"); +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info) +{ + this->pubs.clear(); + this->sessions.clear(); + + for (auto key : _data->frame.Keys()) + { + this->pubs.emplace(key, node.Advertise(key)); + gz::msgs::Float_V msg; + this->floatFields.emplace(key, msg); + const double time = std::chrono::duration(_info.simTime).count(); + auto sess = _data->frame[key].CreateSession(time); + if (!_data->frame[key].IsValid(sess)) + { + gzerr << key << "data is out of time bounds. Nothing will be published" + <finishedTime = true; + continue; + } + this->sessions.emplace(key, sess); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::FileReloaded() +{ + this->finishedTime = false; +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples) +{ + if (this->finishedTime) + { + return; + } + auto now = std::chrono::steady_clock::now(); + std::chrono::duration dt(now - this->lastTick); + + if (this->resample) + { + this->CreatePointCloudTopics(_data, _info); + if (this->finishedTime) { + this->resample = false; + return; + } + this->ResizeCloud(_data, _ecm, _xSamples, _ySamples, _zSamples); + this->resample = false; + this->lastTick = now; + } + + // Progress session pointers to next time stamp + for (auto &it : this->sessions) + { + auto res = + _data->frame[it.first].StepTo(it.second, + std::chrono::duration(_info.simTime).count()); + if (res.has_value()) + { + it.second = res.value(); + } + else + { + gzerr << "Data does not exist beyond this time." + << " Not publishing new environment visuallization data." + << std::endl; + this->finishedTime = true; + return; + } + } + + // Publish at 2 hz for now. In future make reconfigureable. + if (dt.count() > 0.5 && !this->finishedTime) + { + this->Visualize(_data, _xSamples, _ySamples, _zSamples); + this->Publish(); + lastTick = now; + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Visualize( + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples) +{ + for (auto key : _data->frame.Keys()) + { + const auto session = this->sessions[key]; + auto frame = _data->frame[key]; + auto [lower_bound, upper_bound] = frame.Bounds(session); + auto step = upper_bound - lower_bound; + auto dx = step.X() / _xSamples; + auto dy = step.Y() / _ySamples; + auto dz = step.Z() / _zSamples; + std::size_t idx = 0; + for (std::size_t x_steps = 0; x_steps < ceil(_xSamples); x_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < ceil(_ySamples); y_steps++) + { + auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < ceil(_zSamples); z_steps++) + { + auto z = lower_bound.Z() + z_steps * dz; + auto res = frame.LookUp(session, math::Vector3d(x, y, z)); + + if (res.has_value()) + { + this->floatFields[key].mutable_data()->Set(idx, + static_cast(res.value())); + } + else + { + this->floatFields[key].mutable_data()->Set(idx, std::nanf("")); + } + idx++; + } + } + } + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::Publish() +{ + pcPub.Publish(this->pcMsg); + for (auto &[key, pub] : this->pubs) + { + pub.Publish(this->floatFields[key]); + } +} + +///////////////////////////////////////////////// +void EnvironmentVisualizationTool::ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _numXSamples, + unsigned int _numYSamples, + unsigned int _numZSamples) +{ + assert(pubs.size() > 0); + + // Assume all data have same point cloud. + gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, + {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); + auto numberOfPoints = _numXSamples * _numYSamples * _numZSamples; + std::size_t dataSize{ + static_cast(numberOfPoints * pcMsg.point_step())}; + pcMsg.mutable_data()->resize(dataSize); + pcMsg.set_height(1); + pcMsg.set_width(numberOfPoints); + + auto session = this->sessions[this->pubs.begin()->first]; + auto frame = _data->frame[this->pubs.begin()->first]; + auto [lower_bound, upper_bound] = frame.Bounds(session); + + auto step = upper_bound - lower_bound; + auto dx = step.X() / _numXSamples; + auto dy = step.Y() / _numYSamples; + auto dz = step.Z() / _numZSamples; + + // Populate point cloud + gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (std::size_t x_steps = 0; x_steps < _numXSamples; x_steps++) + { + auto x = lower_bound.X() + x_steps * dx; + for (std::size_t y_steps = 0; y_steps < _numYSamples; y_steps++) + { + auto y = lower_bound.Y() + y_steps * dy; + for (std::size_t z_steps = 0; z_steps < _numZSamples; z_steps++) + { + auto z = lower_bound.Z() + z_steps * dz; + auto coords = getGridFieldCoordinates(_ecm, math::Vector3d{x, y, z}, + _data); + + if (!coords.has_value()) + { + continue; + } + + auto pos = coords.value(); + *xIter = pos.X(); + *yIter = pos.Y(); + *zIter = pos.Z(); + ++xIter; + ++yIter; + ++zIter; + } + } + } + for (auto key : _data->frame.Keys()) + { + this->floatFields[key].mutable_data()->Resize( + numberOfPoints, std::nanf("")); + } +} diff --git a/src/systems/environment_preload/VisualizationTool.hh b/src/systems/environment_preload/VisualizationTool.hh new file mode 100644 index 0000000000..bb2e2fce1b --- /dev/null +++ b/src/systems/environment_preload/VisualizationTool.hh @@ -0,0 +1,143 @@ +/* + * 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_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ +#define GZ_SIM_SYSTEMS_ENVIRONMENTPRELOAD_VIZTOOL_HH_ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/components/Environment.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +namespace gz +{ +namespace sim +{ +inline namespace GZ_SIM_VERSION_NAMESPACE +{ + +/// \brief This class helps handle point cloud visuallizations +/// of environment data. +class EnvironmentVisualizationTool +{ + /// \brief Environment constructor + public: EnvironmentVisualizationTool(); + + /// \brief To synchronize member access. + private: std::mutex mutex; + + /// \brief First load we need to scan for existing data sensor + private: bool first{true}; + + /// \brief Enable resampling + public: std::atomic resample{true}; + + /// \brief Time has come to an end. + private: bool finishedTime{false}; + + /// \brief Create publisher structures whenever a new environment is made + /// available. + /// \param[in] _data Data to be visuallized + /// \param[in] _info simulation info for current time step + private: void CreatePointCloudTopics( + const std::shared_ptr &_data, + const UpdateInfo &_info); + + /// \brief Invoke when new file is made available. + public: void FileReloaded(); + + /// \brief Step the visualizations + /// \param[in] _info The simulation info including timestep + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + public: void Step( + const UpdateInfo &_info, + const EntityComponentManager &_ecm, + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Publishes a sample of the data + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void Visualize( + const std::shared_ptr &_data, + double _xSamples, double _ySamples, double _zSamples); + + /// \brief Get the point cloud data. + private: void Publish(); + + /// \brief Resize the point cloud structure (used to reallocate + /// memory when resolution changes) + /// \param[in] _ecm The Entity-Component-Manager + /// \param[in] _data The data to be visuallized + /// \param[in] _xSample Samples along x + /// \param[in] _ySample Samples along y + /// \param[in] _zSample Samples along z + private: void ResizeCloud( + const std::shared_ptr &_data, + const EntityComponentManager &_ecm, + unsigned int _xSamples, unsigned int _ySamples, unsigned int _zSamples); + + /// \brief Publisher for point clouds + private: transport::Node::Publisher pcPub; + + /// \brief Publishers for data + private: std::unordered_map pubs; + + /// \brief Floating point message buffers + private: std::unordered_map floatFields; + + /// \brief GZ buffers + private: transport::Node node; + + /// \brief Point cloud buffer + private: gz::msgs::PointCloudPacked pcMsg; + + /// \brief Session cursors + private: std::unordered_map> sessions; + + /// \brief Duration from last update + private: std::chrono::time_point lastTick; +}; +} +} +} +#endif diff --git a/src/systems/follow_actor/FollowActor.hh b/src/systems/follow_actor/FollowActor.hh index 7a01a920cc..27076786c5 100644 --- a/src/systems/follow_actor/FollowActor.hh +++ b/src/systems/follow_actor/FollowActor.hh @@ -35,24 +35,23 @@ namespace systems /// \class FollowActor FollowActor.hh gz/sim/systems/FollowActor.hh /// \brief Make an actor follow a target entity in the world. /// - /// ## SDF parameters + /// ## System Parameters /// - /// : Name of entity to follow. + /// - ``: Name of entity to follow. /// - /// : Distance in meters to keep from target's origin. + /// - ``: Distance in meters to keep from target's origin. /// - /// : Distance in meters from target's origin when to stop - /// following. When the actor is back within range it starts - /// following again. + /// - ``: Distance in meters from target's origin when to stop + /// following. When the actor is back within range it starts following + /// again. /// - /// : Actor's velocity in m/s + /// - ``: Actor's velocity in m/s /// - /// : Actor's animation to play. If empty, the first animation in - /// the model will be used. + /// - ``: Actor's animation to play. If empty, the first animation + /// in the model will be used. /// - /// : Velocity of the animation on the X axis. Used to - /// coordinate translational motion with the actor's - /// animation. + /// - ``: Velocity of the animation on the X axis. Used to + /// coordinate translational motion with the actor's animation. class FollowActor: public System, public ISystemConfigure, diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index aab1a7cdb0..1ada82b467 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -40,7 +40,7 @@ namespace systems /// forces like linear and quadratic drag, buoyancy (not provided by this /// plugin), etc. /// - /// # System Parameters + /// ## System Parameters /// 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. diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh index ae65473423..ec1b3bd381 100644 --- a/src/systems/joint_controller/JointController.hh +++ b/src/systems/joint_controller/JointController.hh @@ -37,56 +37,60 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the joint to control. Required parameter. + /// - `` The name of the joint to control. Required parameter. /// Can also include multiple `` for identical joints. /// - /// `` True to enable the controller implementation + /// - `` True to enable the controller implementation /// using force commands. If this parameter is not set or is false, the /// controller will use velocity commands internally. /// - /// `` True to enable the use of actuator message + /// - `` True to enable the use of actuator message /// for velocity command. The actuator msg is an array of floats for /// position, velocity and normalized commands. Relies on /// `` for the index of the velocity actuator and /// defaults to topic "/actuators" when `topic` or `subtopic not set. /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the velocity actuator. /// - /// `` Topic to receive commands in. Defaults to + /// - `` Topic to receive commands in. Defaults to /// `/model//joint//cmd_vel`. /// - /// `` Sub topic to receive commands in. - /// Defaults to "/model//". + /// - `` Sub topic to receive commands in. + /// Defaults to "/model//". /// - /// `` Velocity to start with. + /// - `` Velocity to start with. /// - /// ### Velocity mode: No additional parameters are required. + /// ### Velocity mode /// - /// ### Force mode: The controller accepts the next optional parameters: + /// No additional parameters are required. /// - /// `` The proportional gain of the PID. - /// The default value is 1. + /// ### Force mode /// - /// `` The integral gain of the PID. - /// The default value is 0. + /// The controller accepts the next optional parameters: /// - /// `` The derivative gain of the PID. - /// The default value is 0. + /// - `` The proportional gain of the PID. + /// The default value is 1. /// - /// `` The integral upper limit of the PID. - /// The default value is 1. + /// - `` The integral gain of the PID. + /// The default value is 0. + /// + /// - `` The derivative gain of the PID. + /// The default value is 0. + /// + /// - `` The integral upper limit of the PID. + /// The default value is 1. /// - /// `` The integral lower limit of the PID. - /// The default value is -1. + /// - `` The integral lower limit of the PID. + /// The default value is -1. /// - /// `` Output max value of the PID. - /// The default value is 1000. + /// - `` Output max value of the PID. + /// The default value is 1000. /// - /// `` Output min value of the PID. - /// The default value is -1000. + /// - `` Output min value of the PID. + /// The default value is -1000. /// - /// `` Command offset (feed-forward) of the PID. + /// - `` Command offset (feed-forward) of the PID. /// The default value is 0. class JointController : public System, diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index af139641d7..f7290fea29 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -46,56 +46,56 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the joint to control. Required parameter. - /// Can also include multiple `` for identical joints. + /// - `` The name of the joint to control. Required parameter. + /// Can also include multiple `` for identical joints. /// - /// `` Axis of the joint to control. Optional parameter. - /// The default value is 0. + /// - `` Axis of the joint to control. Optional parameter. + /// The default value is 0. /// - /// `` True to enable the use of actutor message + /// - `` True to enable the use of actutor message /// for position command. Relies on `` for the /// index of the position actuator and defaults to topic "/actuators". /// - /// `` used with `` to set + /// - `` used with `` to set /// the index of the position actuator. /// - /// `` The proportional gain of the PID. Optional parameter. - /// The default value is 1. + /// - `` The proportional gain of the PID. Optional parameter. + /// The default value is 1. /// - /// `` The integral gain of the PID. Optional parameter. - /// The default value is 0.1. + /// - `` The integral gain of the PID. Optional parameter. + /// The default value is 0.1. /// - /// `` The derivative gain of the PID. Optional parameter. - /// The default value is 0.01 + /// - `` The derivative gain of the PID. Optional parameter. + /// The default value is 0.01 /// - /// `` The integral upper limit of the PID. Optional parameter. - /// The default value is 1. + /// - `` The integral upper limit of the PID. Optional parameter. + /// The default value is 1. /// - /// `` The integral lower limit of the PID. Optional parameter. - /// The default value is -1. + /// - `` The integral lower limit of the PID. Optional parameter. + /// The default value is -1. /// - /// `` Output max value of the PID. Optional parameter. - /// The default value is 1000. + /// - `` Output max value of the PID. Optional parameter. + /// The default value is 1000. /// - /// `` Output min value of the PID. Optional parameter. - /// The default value is -1000. + /// - `` Output min value of the PID. Optional parameter. + /// The default value is -1000. /// - /// `` Command offset (feed-forward) of the PID. Optional + /// - `` Command offset (feed-forward) of the PID. Optional /// parameter. The default value is 0. /// - /// `` Bypasses the PID and creates a perfect + /// - `` Bypasses the PID and creates a perfect /// position. The maximum speed on the joint can be set using the `` /// tag. /// - /// `` If you wish to listen on a non-default topic you may specify it - /// here, otherwise the controller defaults to listening on + /// - `` If you wish to listen on a non-default topic you may specify + /// it here, otherwise the controller defaults to listening on /// "/model//joint///cmd_pos". /// - /// `` If you wish to listen on a sub_topic you may specify it + /// - `` If you wish to listen on a sub_topic you may specify it /// here "/model//". /// - /// `` Initial position of a joint. Optional parameter. - /// The default value is 0. + /// - `` Initial position of a joint. Optional parameter. + /// The default value is 0. class JointPositionController : public System, public ISystemConfigure, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 28d2408091..b92da24414 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -41,12 +41,13 @@ namespace systems /// a model. Use the `` system parameter, described below, to /// control which joints are published. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the topic to publish to. This parameter is optional, + /// - ``: Name of the topic to publish to. This parameter is optional, /// and if not provided, the joint state will be published to /// "/world//model//joint_state". - /// ``: Name of a joint to publish. This parameter can be + /// + /// - ``: Name of a joint to publish. This parameter can be /// specified multiple times, and is optional. All joints in a model will /// be published if joint names are not specified. class JointStatePublisher diff --git a/src/systems/joint_traj_control/JointTrajectoryController.hh b/src/systems/joint_traj_control/JointTrajectoryController.hh index 09dc7d9e48..5c654e04e0 100644 --- a/src/systems/joint_traj_control/JointTrajectoryController.hh +++ b/src/systems/joint_traj_control/JointTrajectoryController.hh @@ -65,68 +65,68 @@ namespace systems /// /// ## System Parameters /// - /// `` The name of the topic that this plugin subscribes to. + /// - `` The name of the topic that this plugin subscribes to. /// Optional parameter. /// Defaults to "/model/${MODEL_NAME}/joint_trajectory". /// - /// `` If enabled, trajectory execution begins at the + /// - `` If enabled, trajectory execution begins at the /// timestamp contained in the header of received trajectory messages. /// Optional parameter. /// Defaults to false. /// - /// `` Name of a joint to control. + /// - `` Name of a joint to control. /// This parameter can be specified multiple times, i.e. once for each joint. /// Optional parameter. /// Defaults to all 1-axis joints contained in the model SDF (order is kept). /// - /// `` Initial position of a joint (for position control). + /// - `` Initial position of a joint (for position control). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// Defaults to 0 for all joints. /// - /// `<%s_p_gain>` The proportional gain of the PID. + /// - `<%s_p_gain>` The proportional gain of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_i_gain>` The integral gain of the PID. Optional parameter. + /// - `<%s_i_gain>` The integral gain of the PID. Optional parameter. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_d_gain>` The derivative gain of the PID. + /// - `<%s_d_gain>` The derivative gain of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (disabled). /// - /// `<%s_i_min>` The integral lower limit of the PID. + /// - `<%s_i_min>` The integral lower limit of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (no limit if higher than `%s_i_max`). /// - /// `<%s_i_max>` The integral upper limit of the PID. + /// - `<%s_i_max>` The integral upper limit of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is -1 (no limit if lower than `%s_i_min`). /// - /// `<%s_cmd_min>` Output min value of the PID. + /// - `<%s_cmd_min>` Output min value of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is 0 (no limit if higher than `%s_i_max`). /// - /// `<%s_cmd_max>` Output max value of the PID. + /// - `<%s_cmd_max>` Output max value of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. /// The default value is -1 (no limit if lower than `%s_i_min`). /// - /// `<%s_cmd_offset>` Command offset (feed-forward) of the PID. + /// - `<%s_cmd_offset>` Command offset (feed-forward) of the PID. /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). /// This parameter can be specified multiple times. Follows joint_name order. /// Optional parameter. diff --git a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh index c8d0020535..aae88a94eb 100644 --- a/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh +++ b/src/systems/kinetic_energy_monitor/KineticEnergyMonitor.hh @@ -39,20 +39,20 @@ namespace systems /// that surpasses a specific threshold. /// This system can be used to detect when a model could be damaged. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the link to monitor. This name must match + /// - ``: Name of the link to monitor. This name must match /// a name of link within the model. /// - /// ``: Threshold, in Joule (J), after which + /// - ``: Threshold, in Joule (J), after which /// a message is generated on `` with the kinetic energy value that /// surpassed the threshold. /// - /// ``: Custom topic that this system will publish to when kinetic + /// - ``: Custom topic that this system will publish to when kinetic /// energy surpasses the threshold. This element if optional, and the /// default value is `/model/{name_of_model}/kinetic_energy`. /// - /// # Example Usage + /// ## Example Usage /// /** \verbatim diff --git a/src/systems/lens_flare/LensFlare.hh b/src/systems/lens_flare/LensFlare.hh index 95ed7a07d4..9ea069d009 100644 --- a/src/systems/lens_flare/LensFlare.hh +++ b/src/systems/lens_flare/LensFlare.hh @@ -32,23 +32,26 @@ namespace systems /// \brief Private data class for LensFlare Plugin class LensFlarePrivate; - /** \class LensFlare LensFlare.hh \ + /** \class LensFlare LensFlare.hh \ * gz/sim/systems/LensFlare.hh **/ /// \brief Add lens flare effects to the camera output as a render pass - /// The system takes in the following parameter: - /// Sets the scale of the lens flare. If this is - /// not specified, the value defaults to 1.0 /// - /// Sets the color of the lens flare. The default - /// is {1.4, 1.2, 1.0} + /// ## System Parameters /// - /// Sets the number of steps to take in - /// each direction to check for occlusions. - /// The default value is set to 10. Use 0 to disable - /// Sets the light associated with the lens flares. - /// If not specified. The first light in the scene will - /// be used. + /// - ``: Sets the scale of the lens flare. If this is + /// not specified, the value defaults to 1.0 + /// + /// - ``: Sets the color of the lens flare. The default + /// is {1.4, 1.2, 1.0} + /// + /// - ``: Sets the number of steps to take in + /// each direction to check for occlusions. + /// The default value is set to 10. Use 0 to disable + /// + /// - ``: Sets the light associated with the lens flares. + /// If not specified. The first light in the scene will + /// be used. class LensFlare: public System, diff --git a/src/systems/lift_drag/LiftDrag.cc b/src/systems/lift_drag/LiftDrag.cc index fc19a645de..483c1fadd0 100644 --- a/src/systems/lift_drag/LiftDrag.cc +++ b/src/systems/lift_drag/LiftDrag.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -40,6 +41,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ExternalWorldWrenchCmd.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Wind.hh" using namespace gz; using namespace sim; @@ -87,6 +89,10 @@ class gz::sim::systems::LiftDragPrivate /// \brief Cm-alpha rate after stall public: double cmaStall = 0.0; + /// \brief How much Cm changes with a change in control + /// surface deflection angle + public: double cm_delta = 0.0; + /// \brief air density /// at 25 deg C it's about 1.1839 kg/m^3 /// At 20 °C and 101.325 kPa, dry air has a density of 1.2041 kg/m3. @@ -155,6 +161,7 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, this->area = _sdf->Get("area", this->area).first; this->alpha0 = _sdf->Get("a0", this->alpha0).first; this->cp = _sdf->Get("cp", this->cp).first; + this->cm_delta = _sdf->Get("cm_delta", this->cm_delta).first; // blade forward (-drag) direction in link frame this->forward = @@ -206,7 +213,6 @@ void LiftDragPrivate::Load(const EntityComponentManager &_ecm, return; } - if (_sdf->HasElement("control_joint_name")) { auto controlJointName = _sdf->Get("control_joint_name"); @@ -259,6 +265,13 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto worldPose = _ecm.Component(this->linkEntity); + // get wind as a component from the _ecm + components::WorldLinearVelocity *windLinearVel = nullptr; + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + windLinearVel = + _ecm.Component(windEntity); + } components::JointPosition *controlJointPosition = nullptr; if (this->controlJointEntity != kNullEntity) { @@ -271,7 +284,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) const auto &pose = worldPose->Data(); const auto cpWorld = pose.Rot().RotateVector(this->cp); - const auto vel = worldLinVel->Data() + worldAngVel->Data().Cross(cpWorld); + auto vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld); + if (windLinearVel != nullptr){ + vel = worldLinVel->Data() + worldAngVel->Data().Cross( + cpWorld) - windLinearVel->Data(); + } if (vel.Length() <= 0.01) return; @@ -281,6 +299,12 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // rotate forward and upward vectors into world frame const auto forwardI = pose.Rot().RotateVector(this->forward); + if (forwardI.Dot(vel) <= 0.0){ + // Only calculate lift or drag if the wind relative velocity + // is in the same direction + return; + } + math::Vector3d upwardI; if (this->radialSymmetry) { @@ -304,7 +328,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) spanwiseI.Dot(velI), minRatio, maxRatio); // get cos from trig identity - const double cosSweepAngle = 1.0 - sinSweepAngle * sinSweepAngle; + double cosSweepAngle = sqrt(1.0 - sinSweepAngle * sinSweepAngle); double sweep = std::asin(sinSweepAngle); // truncate sweep to within +/-90 deg @@ -336,7 +360,7 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) // compute angle between upwardI and liftI // in general, given vectors a and b: - // cos(theta) = a.Dot(b)/(a.Length()*b.Lenghth()) + // cos(theta) = a.Dot(b)/(a.Length()*b.Length()) // given upwardI and liftI are both unit vectors, we can drop the denominator // cos(theta) = a.Dot(b) const double cosAlpha = @@ -435,15 +459,16 @@ void LiftDragPrivate::Update(EntityComponentManager &_ecm) else cm = this->cma * alpha * cosSweepAngle; - /// \todo(anyone): implement cm - /// for now, reset cm to zero, as cm needs testing - cm = 0.0; + // Take into account the effect of control surface deflection angle to cm + if (controlJointPosition && !controlJointPosition->Data().empty()) + { + cm += this->cm_delta * controlJointPosition->Data()[0]; + } // compute moment (torque) at cp // spanwiseI used to be momentDirection math::Vector3d moment = cm * q * this->area * spanwiseI; - // force and torque about cg in world frame math::Vector3d force = lift + drag; math::Vector3d torque = moment; @@ -530,7 +555,6 @@ void LiftDrag::PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) this->dataPtr->Load(_ecm, this->dataPtr->sdfConfig); this->dataPtr->initialized = true; - if (this->dataPtr->validConfig) { Link link(this->dataPtr->linkEntity); diff --git a/src/systems/lift_drag/LiftDrag.hh b/src/systems/lift_drag/LiftDrag.hh index 8c7cba996d..afc4ee178d 100644 --- a/src/systems/lift_drag/LiftDrag.hh +++ b/src/systems/lift_drag/LiftDrag.hh @@ -35,34 +35,37 @@ namespace systems /// \brief The LiftDrag system computes lift and drag forces enabling /// simulation of aerodynamic robots. /// - /// The following parameters are used by the system: + /// ## System Parameters /// - /// link_name : Name of the link affected by the group of lift/drag - /// properties. This can be a scoped name to reference links in - /// nested models. \sa entitiesFromScopedName to learn more - /// about scoped names. - /// air_density : Density of the fluid this model is suspended in. - /// area : Surface area of the link. - /// a0 : The initial "alpha" or initial angle of attack. a0 is also - /// the y-intercept of the alpha-lift coefficient curve. - /// cla : The ratio of the coefficient of lift and alpha slope before - /// stall. Slope of the first portion of the alpha-lift - /// coefficient curve. - /// cda : The ratio of the coefficient of drag and alpha slope before - /// stall. - /// cp : Center of pressure. The forces due to lift and drag will be - /// applied here. - /// forward : 3-vector representing the forward direction of motion in the - /// link frame. - /// upward : 3-vector representing the direction of lift or drag. - /// alpha_stall : Angle of attack at stall point; the peak angle of attack. - /// cla_stall : The ratio of coefficient of lift and alpha slope after - /// stall. Slope of the second portion of the alpha-lift - /// coefficient curve. - /// cda_stall : The ratio of coefficient of drag and alpha slope after - /// stall. - /// control_joint_name: Name of joint that actuates a control surface for this - /// lifting body (Optional) + /// - ``: Name of the link affected by the group of lift/drag + /// properties. This can be a scoped name to reference links in + /// nested models. \sa entitiesFromScopedName to learn more + /// about scoped names. + /// - ``: Density of the fluid this model is suspended in. + /// - ``: Surface area of the link. + /// - ``: The initial "alpha" or initial angle of attack. a0 is also + /// the y-intercept of the alpha-lift coefficient curve. + /// - ``: The ratio of the coefficient of lift and alpha slope before + /// stall. Slope of the first portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of the coefficient of drag and alpha slope before + /// stall. + /// - ``: Center of pressure. The forces due to lift and drag will be + /// applied here. + /// - ``: 3-vector representing the forward direction of motion in + /// the link frame. + /// - ``: 3-vector representing the direction of lift or drag. + /// - ``: Angle of attack at stall point; the peak angle of + /// attack. + /// - ``: The ratio of coefficient of lift and alpha slope after + /// stall. Slope of the second portion of the alpha-lift + /// coefficient curve. + /// - ``: The ratio of coefficient of drag and alpha slope after + /// stall. + /// - ``: Name of joint that actuates a control surface + /// for this lifting body (Optional) + /// - ``: How much Cm changes with a change in control + /// surface deflection angle class LiftDrag : public System, public ISystemConfigure, diff --git a/src/systems/log/LogPlayback.hh b/src/systems/log/LogPlayback.hh index bc8fd28110..66712a9c89 100644 --- a/src/systems/log/LogPlayback.hh +++ b/src/systems/log/LogPlayback.hh @@ -33,8 +33,9 @@ namespace systems // Forward declarations. class LogPlaybackPrivate; - /// \class LogPlayback LogPlayback.hh - /// gz/sim/systems/log/LogPlayback.hh + /** \class LogPlayback LogPlayback.hh \ + * gz/sim/systems/log/LogPlayback.hh + **/ /// \brief Log state playback class LogPlayback: public System, diff --git a/src/systems/log_video_recorder/LogVideoRecorder.hh b/src/systems/log_video_recorder/LogVideoRecorder.hh index 958f0152ce..0de7eb46ac 100644 --- a/src/systems/log_video_recorder/LogVideoRecorder.hh +++ b/src/systems/log_video_recorder/LogVideoRecorder.hh @@ -38,18 +38,21 @@ namespace systems /// \brief System which recordings videos from log playback /// There are two ways to specify what entities in the log playback to follow /// and record videos for: 1) by entity name and 2) by region. See the - /// following parameters: - /// - `` Name of entity to record. - /// - `` Axis-aligned box where entities are at start of log - /// + `` Min corner position of box region. - /// + `` Max corner position of box region. - /// - `` Sim time when recording should start - /// - `` Sim time when recording should end - /// - `` Exit gz-sim when log playback recording ends + /// system parameters. /// /// When recording is finished. An `end` string will be published to the /// `/log_video_recorder/status` topic and the videos are saved to a /// timestamped directory + /// + /// ## System Parameters + /// + /// - `` Name of entity to record. + /// - `` Axis-aligned box where entities are at start of log + /// + `` Min corner position of box region. + /// + `` Max corner position of box region. + /// - `` Sim time when recording should start + /// - `` Sim time when recording should end + /// - `` Exit gz-sim when log playback recording ends class LogVideoRecorder final: public System, public ISystemConfigure, diff --git a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh index 04c93edd45..0a4873d474 100644 --- a/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh +++ b/src/systems/logical_audio_sensor_plugin/LogicalAudioSensorPlugin.hh @@ -42,6 +42,8 @@ namespace systems /// such as speakers. This plugin is meant to check if audio /// could theoretically be heard at a certain location or not. /// + /// ## System Parameters + /// /// Secifying an audio source via SDF is done as follows: /// /// - `` A new audio source in the environment, which has the diff --git a/src/systems/mecanum_drive/MecanumDrive.hh b/src/systems/mecanum_drive/MecanumDrive.hh index 4cc910eee1..1442967687 100644 --- a/src/systems/mecanum_drive/MecanumDrive.hh +++ b/src/systems/mecanum_drive/MecanumDrive.hh @@ -35,58 +35,59 @@ namespace systems /// \brief Mecanum drive controller which can be attached to a model /// with any number of front/back left/right wheels. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of a joint that controls a front left wheel. + /// - ``: Name of a joint that controls a front left wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a front right wheel. - /// This element can appear multiple times, and must appear at least once. + /// - ``: Name of a joint that controls a front right + /// wheel. This element can appear multiple times, and must appear at least + /// once. /// - /// ``: Name of a joint that controls a back left wheel. + /// - ``: Name of a joint that controls a back left wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Name of a joint that controls a back right wheel. + /// - ``: Name of a joint that controls a back right wheel. /// This element can appear multiple times, and must appear at least once. /// - /// ``: Longitudinal distance between front and back wheels, + /// - ``: Longitudinal distance between front and back wheels, /// in meters. This element is optional, although it is recommended to be /// included with an appropriate value. The default value is 1.0m. /// - /// ``: Lateral distance between left and right wheels, + /// - ``: Lateral distance between left and right wheels, /// in meters. This element is optional, although it is recommended to be /// included with an appropriate value. The default value is 1.0m. /// - /// ``: Wheel radius in meters. This element is optional, + /// - ``: Wheel radius in meters. This element is optional, /// although it is recommended to be included with an appropriate value. The /// default value is 0.2m. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic that this system will subscribe to in order to + /// - ``: Custom topic that this system will subscribe to in order to /// receive command velocity messages. This element if optional, and the /// default value is `/model/{name_of_model}/cmd_vel`. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `frame_id` to `child_frame_id`. This element if optional, /// and the default value is `/model/{name_of_model}/tf`. /// - /// ``: Custom `frame_id` field that this system will use as the + /// - ``: Custom `frame_id` field that this system will use as the /// origin of the odometry transform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, and the /// default value is `{name_of_model}/odom`. /// - /// ``: Custom `child_frame_id` that this system will use as + /// - ``: Custom `child_frame_id` that this system will use as /// the target of the odometry trasnform in both the `` /// `gz.msgs.Pose_V` message and the `` /// `gz.msgs.Odometry` message. This element if optional, - /// and the default value is `{name_of_model}/{name_of_link}`. + /// and the default value is `{name_of_model}/{name_of_link}`. class MecanumDrive : public System, public ISystemConfigure, diff --git a/src/systems/multicopter_control/MulticopterVelocityControl.hh b/src/systems/multicopter_control/MulticopterVelocityControl.hh index c414d1ec53..eb2cd1caf8 100644 --- a/src/systems/multicopter_control/MulticopterVelocityControl.hh +++ b/src/systems/multicopter_control/MulticopterVelocityControl.hh @@ -69,84 +69,85 @@ namespace systems /// * Maximum acceleration limit /// * Can be enabled/disabled at runtime. /// - /// # Parameters - /// The following parameters are used by the system + /// ## System Parameters /// - /// robotNamespace: All gz-transport topics subscribed to and published by - /// the system will be prefixed by this string. This is a required parameter. + /// - `robotNamespace`: All gz-transport topics subscribed to and published by + /// the system will be prefixed by this string. This is a required parameter. /// - /// commandSubTopic: The system subscribes to this topic to receive twist + /// - `commandSubTopic`: The system subscribes to this topic to receive twist /// commands. The default value is "cmd_vel". /// - /// enableSubTopic: Topic to enable or disable the system. If false, the + /// - `enableSubTopic`: Topic to enable or disable the system. If false, the /// controller sends a zero rotor velocity command once and gets disabled. If /// the vehicle is in the air, disabling the controller will cause it to fall. /// If true, the controller becomes enabled and waits for a twist message. The /// default value is "enable". /// - /// comLinkName: The link associated with the center of mass of the vehicle. - /// That is, the origin of the center of mass may not be on this link, but - /// this link and the center of mass frame have a fixed transform. Almost - /// always this should be the base_link of the vehicle. This is a required - /// parameter. + /// - `comLinkName`: The link associated with the center of mass of the + /// vehicle. That is, the origin of the center of mass may not be on this + /// link, but this link and the center of mass frame have a fixed transform. + /// Almost always this should be the base_link of the vehicle. This is a + /// required parameter. /// - /// velocityGain (x, y, z): Proportional gain on linear velocity. + /// - `velocityGain` (x, y, z): Proportional gain on linear velocity. /// attitudeGain (roll, pitch, yaw): Proportional gain on attitude. This /// parameter is scaled by the inverse of the inertia matrix so two vehicles /// with different inertial characteristics may have the same gain if other /// parameters, such as the forceConstant, are kept the same. This is a /// required parameter. /// - /// angularRateGain (roll, pitch, yaw): Proportional gain on angular velocity. - /// Even though only the yaw angle velocity is controlled, proper gain values - /// for roll and pitch velocities must be specified. This parameter is scaled - /// by the inverse of the inertia matrix so two vehicles with different - /// inertial characteristics may have the same gain if other parameters, such - /// as the forceConstant, are kept the same. This is a required parameter. + /// - `angularRateGain` (roll, pitch, yaw): Proportional gain on angular + /// velocity. Even though only the yaw angle velocity is controlled, proper + /// gain values for roll and pitch velocities must be specified. This + /// parameter is scaled by the inverse of the inertia matrix so two vehicles + /// with different inertial characteristics may have the same gain if other + /// parameters, such as the forceConstant, are kept the same. This is a + /// required parameter. /// - /// maxLinearAcceleration (x, y, z): Maximum limit on linear acceleration. + /// - `maxLinearAcceleration` (x, y, z): Maximum limit on linear acceleration. /// The default value is DBL_MAX. /// - /// maximumLinearVelocity (x, y, z): Maximum commanded linear velocity. The - /// default value is DBL_MAX. - - /// maximumAngularVelocity (roll, pitch, yaw): Maximum commanded angular + /// - `maximumLinearVelocity` (x, y, z): Maximum commanded linear velocity. + /// The default value is DBL_MAX. + /// + /// - `maximumAngularVelocity` (roll, pitch, yaw): Maximum commanded angular /// velocity. The default value is DBL_MAX. /// - /// linearVelocityNoiseMean (x, y, z): Mean of Gaussian noise on linear + /// - `linearVelocityNoiseMean` (x, y, z): Mean of Gaussian noise on linear /// velocity values obtained from simulation. The default value is (0, 0, 0). /// - /// linearVelocityNoiseStdDev (x, y, z): Standard deviation of Gaussian noise - /// on linear values obtained from simulation. A value of 0 implies noise is - /// NOT applied to the component. The default value is (0, 0, 0). + /// - `linearVelocityNoiseStdDev` (x, y, z): Standard deviation of Gaussian + /// noise on linear values obtained from simulation. A value of 0 implies + /// noise is NOT applied to the component. The default value is (0, 0, 0). /// - /// angularVelocityNoiseMean (roll, pitch, yaw): Mean of Gaussian noise on + /// - `angularVelocityNoiseMean` (roll, pitch, yaw): Mean of Gaussian noise on /// angular velocity values obtained from simulation. The default value is (0, /// 0, 0). /// - /// angularVelocityNoiseStdDev (roll, pitch, yaw): Standard deviation of + /// - `angularVelocityNoiseStdDev` (roll, pitch, yaw): Standard deviation of /// gaussian noise on angular velocity values obtained from simulation. A /// value of 0 implies noise is NOT applied to the component. The default /// value is (0, 0, 0). /// - /// rotorConfiguration: This contains a list of `` elements for each - /// rotor in the vehicle. This is a required parameter. + /// - `rotorConfiguration`: This contains a list of `` elements for + /// each rotor in the vehicle. This is a required parameter. /// - /// rotor: Contains information about a rotor in the vehicle. All the + /// - `rotor`: Contains information about a rotor in the vehicle. All the /// elements of `` are required parameters. /// - /// jointName: The name of the joint associated with this rotor. + /// - `jointName`: The name of the joint associated with this rotor. /// - /// forceConstant: A constant that multiplies with the square of the + /// - `forceConstant`: A constant that multiplies with the square of the /// rotor's velocity to compute its thrust. /// - /// momentConstant: A constant the multiplies with the rotor's thrust to - /// compute its moment. + /// - `momentConstant`: A constant the multiplies with the rotor's + /// thrust to compute its moment. + /// + /// - `direction`: Direction of rotation of the rotor. +1 is + /// counterclockwise and -1 is clockwise. /// - /// direction: Direction of rotation of the rotor. +1 is counterclockwise - /// and -1 is clockwise. + /// ## Examples /// - /// # Examples /// See examples/worlds/quadcopter.sdf for a demonstration. /// class MulticopterVelocityControl diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index f7b462da7c..cab9bd6d14 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -36,44 +36,44 @@ namespace systems /// order to periodically publish 2D or 3D odometry data in the form of /// gz::msgs::Odometry messages. /// - /// # System Parameters + /// ## System Parameters /// - /// ``: Name of the world-fixed coordinate frame for the + /// - ``: Name of the world-fixed coordinate frame for the // odometry message. This element is optional, and the default value /// is `{name_of_model}/odom`. /// - /// ``: Name of the coordinate frame rigidly attached + /// - ``: Name of the coordinate frame rigidly attached /// to the mobile robot base. This element is optional, and the default /// value is `{name_of_model}/base_footprint`. /// - /// ``: Odometry publication frequency. This + /// - ``: Odometry publication frequency. This /// element is optional, and the default value is 50Hz. /// - /// ``: Custom topic on which this system will publish odometry + /// - ``: Custom topic on which this system will publish odometry /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/odometry`. /// - /// ``: Custom topic on which this system will publish - /// odometry with covariance messages. This element is optional, and the - /// default value is `/model/{name_of_model}/odometry_with_covariance`. + /// - ``: Custom topic on which this system will + /// publish odometry with covariance messages. This element is optional, and + /// the default value is `/model/{name_of_model}/odometry_with_covariance`. /// - /// ``: Custom topic on which this system will publish the + /// - ``: Custom topic on which this system will publish the /// transform from `odom_frame` to `robot_base_frame`. This element is /// optional, and the default value is `/model/{name_of_model}/pose`. /// - /// ``: Number of dimensions to represent odometry. Only 2 and 3 + /// - ``: Number of dimensions to represent odometry. Only 2 and 3 /// dimensional spaces are supported. This element is optional, and the /// default value is 2. /// - /// ``: Position offset relative to the body fixed frame, the + /// - ``: Position offset relative to the body fixed frame, the /// default value is 0 0 0. This offset will be added to the odometry /// message. /// - /// ``: Rotation offset relative to the body fixed frame, the + /// - ``: Rotation offset relative to the body fixed frame, the /// default value is 0 0 0. This offset will be added to the odometry - /// message. + /// message. /// - /// ``: Standard deviation of the Gaussian noise to be added + /// - ``: Standard deviation of the Gaussian noise to be added /// to pose and twist messages. This element is optional, and the default /// value is 0. class OdometryPublisher diff --git a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh index 664c109412..348cff0acf 100644 --- a/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh +++ b/src/systems/optical_tactile_plugin/OpticalTactilePlugin.hh @@ -37,54 +37,52 @@ namespace systems /// /// It requires that contact sensor and depth camera be placed in at least /// one link on the model on which this plugin is attached. - /// TODO: + /// + /// \todo(anyone) /// Currently, the contacts returned from the physics engine (which tends to /// be sparse) and the normal forces separately computed from the depth /// camera (which is dense, resolution adjustable) are disjoint. It is /// left as future work to combine the two sets of data. /// - /// Parameters: + /// ## System Parameters /// - /// Set this to true so the plugin works from the start and - /// doesn't need to be enabled. This element is optional, and the - /// default value is true. + /// - ``: Set this to true so the plugin works from the start and + /// doesn't need to be enabled. This element is optional, and the + /// default value is true. /// - /// Namespace for transport topics/services. If there are more - /// than one optical tactile plugins, their namespaces should - /// be different. - /// This element is optional, and the default value is - /// "optical_tactile_sensor". - /// //enable : Service used to enable and disable the - /// plugin. - /// //normal_forces : Topic where a message is - /// published each time the - /// normal forces are computed + /// - ``: Namespace for transport topics/services. If there are + /// more than one optical tactile plugins, their namespaces should + /// be different. This element is optional, and the default value is + /// "optical_tactile_sensor". + /// - `//enable`: Service used to enable and disable the plugin. + /// - `//normal_forces`: Topic where a message is + /// published each time the normal forces are computed /// - /// Number n of pixels to skip when visualizing - /// the forces. One vector representing a normal - /// force is computed for every nth pixel. This - /// element must be positive and it is optional. - /// The default value is 30. + /// - ``: Number n of pixels to skip when + /// visualizing the forces. One vector representing a normal + /// force is computed for every nth pixel. This + /// element must be positive and it is optional. + /// The default value is 30. /// - /// Length in meters of the forces visualized if - /// is set to true. This parameter is - /// optional, and the default value is 0.01. + /// - ``: Length in meters of the forces visualized if + /// is set to true. This parameter is + /// optional, and the default value is 0.01. /// - /// Extended sensing distance in meters. The sensor will - /// output data coming from its collision geometry plus - /// this distance. This element is optional, and the - /// default value is 0.001. + /// - ``: Extended sensing distance in meters. The sensor + /// will output data coming from its collision geometry plus + /// this distance. This element is optional, and the + /// default value is 0.001. /// - /// Whether to visualize the sensor or not. This element - /// is optional, and the default value is false. + /// - ``: Whether to visualize the sensor or not. This + /// element is optional, and the default value is false. /// - /// Whether to visualize the contacts from the contact - /// sensor based on physics. This element is optional, - /// and the default value is false. + /// - ``: Whether to visualize the contacts from the + /// contact sensor based on physics. This element is optional, + /// and the default value is false. /// - /// Whether to visualize normal forces computed from the - /// depth camera. This element is optional, and the - /// default value is false. + /// - ``: Whether to visualize normal forces computed from + /// the depth camera. This element is optional, and the + /// default value is false. class OpticalTactilePlugin : public System, diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh index 848fa21a61..f6086bc2bb 100644 --- a/src/systems/particle_emitter/ParticleEmitter.hh +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -32,10 +32,10 @@ namespace systems class ParticleEmitterPrivate; /// \brief A system for running and managing particle emitters. A particle - /// emitter is defined using the SDF element. + /// emitter is defined using the `` SDF element. /// /// This system will create a transport subscriber for each - /// using the child name. If a is not + /// `` using the child `` name. If a `` is not /// specified, the following topic naming scheme will be used: /// `/model/{model_name}/link/{link_name}/particle_emitter/{emitter_name}/cmd` class ParticleEmitter diff --git a/src/systems/performer_detector/PerformerDetector.hh b/src/systems/performer_detector/PerformerDetector.hh index bbdb9e3ddf..cd0c7f6f50 100644 --- a/src/systems/performer_detector/PerformerDetector.hh +++ b/src/systems/performer_detector/PerformerDetector.hh @@ -45,8 +45,9 @@ namespace systems /// PerformerDetector's region, which is also represented by an /// gz::math::AxisAlignedBox. When a performer is detected, the system /// publishes a gz.msgs.Pose message with the pose of the detected - /// performer with respect to the model containing the PerformerDetector. The - /// name and id fields of the Pose message will be set to the name and the + /// performer with respect to the model containing the PerformerDetector. + /// + /// The name and id fields of the Pose message will be set to the name and the /// entity of the detected performer respectively. The header of the Pose /// message contains the time stamp of detection. The `data` field of the /// header will contain the key "frame_id" with a value set to the name of @@ -62,19 +63,22 @@ namespace systems /// The system does not assume that levels are enabled, but it does require /// performers to be specified. /// - /// ## System parameters + /// ## System Parameters /// - /// ``: Custom topic to be used for publishing when a performer is + /// - ``: Custom topic to be used for publishing when a performer is /// detected. If not set, the default topic with the following pattern would /// be used "/model//performer_detector/status". The topic type /// is gz.msgs.Pose - /// ``: Detection region. Currently, only the `` geometry is + /// + /// - ``: Detection region. Currently, only the `` geometry is /// supported. The position of the geometry is derived from the pose of the /// containing model. - /// ``: Additional pose offset relative to the parent model's pose. + /// + /// - ``: Additional pose offset relative to the parent model's pose. /// This pose is added to the parent model pose when computing the /// detection region. Only the position component of the `` is used. - /// ``: Zero or more key-value pairs that will be + /// + /// - ``: Zero or more key-value pairs that will be /// included in the header of the detection messages. A `` /// element should have child `` and `` elements whose /// contents are interpreted as strings. Keys value pairs are stored in a diff --git a/src/systems/physics/Physics.hh b/src/systems/physics/Physics.hh index d7944668e2..cec62d341c 100644 --- a/src/systems/physics/Physics.hh +++ b/src/systems/physics/Physics.hh @@ -64,9 +64,15 @@ namespace systems /// \class Physics Physics.hh gz/sim/systems/Physics.hh /// \brief Base class for a System. - /// Includes optional parameter : . When set + /// + /// ## System Parameters + /// + /// - ``: Optional. When set /// to false, the name of colliding entities is not populated in - /// the contacts. Remains true by default. Usage : + /// the contacts. Remains true by default. + /// + /// ## Example + /// /// ``` /// /pose_static" topic. - /// This will cause only dynamic poses to be - /// published on the "/pose" - /// topic. - /// static_update_frequency : Frequency of static pose publications in Hz. A - /// negative frequency publishes as fast as - /// possible (i.e, at the rate of the simulation - /// step). + /// - ``: Set to true to publish link pose + /// - ``: Set to true to publish visual pose + /// - ``: Set to true to publish collision pose + /// - ``: Set to true to publish sensor pose + /// - ``: Set to true to publish model pose. + /// - ``: Set to true to publish nested model + /// pose. The pose of the model that contains this system is also published + /// unless publish_model_pose is set to false + /// - ``: Set to true to publish a gz::msgs::Pose_V + /// message instead of mulitple gz::msgs::Pose messages. + /// - ``: Frequency of pose publications in Hz. A negative + /// frequency publishes as fast as possible (i.e, at the rate of the + /// simulation step) + /// - ``: Set to true to publish static poses on a + /// "/pose_static" topic. This will cause only dynamic + /// poses to be published on the "/pose" topic. + /// - ``: Frequency of static pose publications in + /// Hz. A negative frequency publishes as fast as possible (i.e, at the + /// rate of the simulation step). class PosePublisher : public System, public ISystemConfigure, diff --git a/src/systems/python_system_loader/PythonSystemLoader.hh b/src/systems/python_system_loader/PythonSystemLoader.hh index 9ba0673fd1..c79f9ac29f 100644 --- a/src/systems/python_system_loader/PythonSystemLoader.hh +++ b/src/systems/python_system_loader/PythonSystemLoader.hh @@ -55,14 +55,14 @@ namespace systems /// check if the corresponding method is implemented in the Python system and /// skip it if it's not found. /// -/// See examples/scripts/python_api/systems/test_system.py for an example +/// See `examples/scripts/python_api/systems/test_system.py` for an example /// -/// ## Parameters -/// * : Name of python module to be loaded. The search path +/// ## System Parameters +/// * `` : Name of python module to be loaded. The search path /// includes `GZ_SIM_SYSTEM_PLUGIN_PATH` as well as /// `PYTHONPATH`. /// -/// The contents of the tag will be available in the configure method +/// The contents of the `` tag will be available in the configure method /// of the Python system // TODO(azeey) Add ParameterConfigure class GZ_SIM_PYTHON_SYSTEM_LOADER_SYSTEM_HIDDEN PythonSystemLoader final diff --git a/src/systems/rf_comms/RFComms.hh b/src/systems/rf_comms/RFComms.hh index 550ca70b12..16ccf0c0cd 100644 --- a/src/systems/rf_comms/RFComms.hh +++ b/src/systems/rf_comms/RFComms.hh @@ -38,31 +38,37 @@ namespace systems /// This communication model has been ported from: /// https://github.com/osrf/subt . /// + /// ## System Parameters + /// /// This system can be configured with the following SDF parameters: /// - /// * Optional parameters: - /// Element used to capture the range configuration based on a - /// log-normal distribution. This block can contain any of the - /// next parameters: - /// * : Hard limit on range (meters). No communication will - /// happen beyond this range. Default is 50. - /// * : Fading exponent used in the normal distribution. - /// Default is 2.5. - /// * : Path loss at the reference distance (1 meter) in dBm. - /// Default is 40. - /// * : Standard deviation of the normal distribution. - /// Default is 10. + /// Optional parameters: + /// + /// - ``: Element used to capture the range configuration based + /// on a log-normal distribution. This block can contain any of the + /// next parameters: + /// - ``: Hard limit on range (meters). No communication will + /// happen beyond this range. Default is 50. + /// - ``: Fading exponent used in the normal distribution. + /// Default is 2.5. + /// - ``: Path loss at the reference distance (1 meter) in dBm. + /// Default is 40. + /// - ``: Standard deviation of the normal distribution. + /// Default is 10. + /// + /// - ``: Element used to capture the radio configuration. + /// This block can contain any of the + /// next parameters: + /// - ``: Capacity of radio in bits-per-second. + /// Default is 54000000 (54 Mbps). + /// - ``: Transmitter power in dBm. Default is 27dBm (500mW). + /// - ``: Noise floor in dBm. Default is -90dBm. + /// - ``: Supported modulations: ["QPSK"]. Default is "QPSK". /// - /// Element used to capture the radio configuration. - /// This block can contain any of the - /// next parameters: - /// * : Capacity of radio in bits-per-second. - /// Default is 54000000 (54 Mbps). - /// * : Transmitter power in dBm. Default is 27dBm (500mW). - /// * : Noise floor in dBm. Default is -90dBm. - /// * : Supported modulations: ["QPSK"]. Default is "QPSK". + /// ## Example /// /// Here's an example: + /// ``` /// @@ -79,6 +85,7 @@ namespace systems /// QPSK /// /// + /// ``` class RFComms : public comms::ICommsModel { diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index d70dd6590a..3ce3f4b619 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -126,6 +126,9 @@ class gz::sim::systems::SensorsPrivate /// \brief Mutex to protect rendering data public: std::mutex renderMutex; + /// \brief Mutex to protect renderUtil changes + public: std::mutex renderUtilMutex; + /// \brief Condition variable to signal rendering thread /// /// This variable is used to block/unblock operations in the rendering @@ -133,6 +136,13 @@ class gz::sim::systems::SensorsPrivate /// documentation on RenderThread. public: std::condition_variable renderCv; + /// \brief Condition variable to signal update time applied + /// + /// This variable is used to block/unblock operations in PostUpdate thread + /// to make sure renderUtil's ECM updates are applied to the scene first + /// before they are overriden by PostUpdate + public: std::condition_variable updateTimeCv; + /// \brief Connection to events::Stop event, used to stop thread public: common::ConnectionPtr stopConn; @@ -142,6 +152,12 @@ class gz::sim::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Update time applied in the rendering thread + public: std::chrono::steady_clock::duration updateTimeApplied; + + /// \brief Update time to be appplied in the rendering thread + public: std::chrono::steady_clock::duration updateTimeToApply; + /// \brief Next sensors update time public: std::chrono::steady_clock::duration nextUpdateTime; @@ -298,13 +314,13 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; - std::chrono::steady_clock::duration updateTimeApplied; GZ_PROFILE("SensorsPrivate::RunOnce"); { GZ_PROFILE("Update"); - std::unique_lock timeLock(this->renderMutex); + std::unique_lock timeLock(this->renderUtilMutex); this->renderUtil.Update(); - updateTimeApplied = this->updateTime; + this->updateTimeApplied = this->updateTime; + this->updateTimeCv.notify_one(); } bool activeSensorsEmpty = true; @@ -335,7 +351,7 @@ void SensorsPrivate::RunOnce() { GZ_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(updateTimeApplied); + this->scene->SetTime(this->updateTimeApplied); // 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 @@ -362,11 +378,11 @@ void SensorsPrivate::RunOnce() // safety check to see if reset occurred while we're rendering // avoid publishing outdated data if reset occurred std::unique_lock timeLock(this->renderMutex); - if (updateTimeApplied <= this->updateTime) + if (this->updateTimeApplied <= this->updateTime) { // publish data GZ_PROFILE("RunOnce"); - this->sensorManager.RunOnce(updateTimeApplied); + this->sensorManager.RunOnce(this->updateTimeApplied); this->eventManager->Emit(); } @@ -625,8 +641,10 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) s->SetNextDataUpdateTime(_info.simTime); } this->dataPtr->nextUpdateTime = _info.simTime; - std::unique_lock lock(this->dataPtr->renderMutex); + std::unique_lock lock2(this->dataPtr->renderUtilMutex); this->dataPtr->updateTime = _info.simTime; + this->dataPtr->updateTimeToApply = _info.simTime; + this->dataPtr->updateTimeApplied = _info.simTime; } } @@ -688,7 +706,19 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { { - std::unique_lock lock(this->dataPtr->renderMutex); + GZ_PROFILE("UpdateFromECM"); + // Make sure we do not override the state in renderUtil if there are + // still ECM changes that still need to be propagated to the scene, + // i.e. wait until renderUtil.Update(), has taken place in the + // rendering thread + std::unique_lock lock(this->dataPtr->renderUtilMutex); + this->dataPtr->updateTimeCv.wait(lock, [this]() + { + return !this->dataPtr->updateAvailable || + (this->dataPtr->updateTimeToApply == + this->dataPtr->updateTimeApplied); + }); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); this->dataPtr->updateTime = _info.simTime; } @@ -739,17 +769,23 @@ void Sensors::PostUpdate(const UpdateInfo &_info, std::unique_lock lockSensors(this->dataPtr->sensorsMutex); this->dataPtr->activeSensors = std::move(this->dataPtr->sensorsToUpdate); + } + + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); - this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( - this->dataPtr->sensorsToUpdate, _info.simTime); + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; - // Force scene tree update if there are sensors to be created or - // subscribes to the render events. This does not necessary force - // sensors to update. Only active sensors will be updated - this->dataPtr->forceUpdate = - (this->dataPtr->renderUtil.PendingSensors() > 0) || - hasRenderConnections; + { + std::unique_lock timeLock(this->dataPtr->renderUtilMutex); + this->dataPtr->updateTimeToApply = this->dataPtr->updateTime; } + { std::unique_lock cvLock(this->dataPtr->renderMutex); this->dataPtr->updateAvailable = true; diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index caf533ce29..41a314e757 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -40,14 +40,14 @@ namespace systems /// /// ## System Parameters /// - /// - `` Name of the render engine, such as 'ogre' or 'ogre2'. - /// - `` Color used for the scene's background. This + /// - ``: Name of the render engine, such as "ogre" or "ogre2". + /// - ``: Color used for the scene's background. This /// will override the background color specified in a world's SDF /// element. This background color is used by sensors, not the GUI. - /// - `` Color used for the scene's ambient light. This + /// - ``: Color used for the scene's ambient light. This /// will override the ambient value specified in a world's SDF /// element. This ambient light is used by sensors, not the GUI. - /// - `` Disable sensors if the model's + /// - ``: Disable sensors if the model's /// battery plugin charge reaches zero. Sensors that are in nested /// models are also affected. /// diff --git a/src/systems/shader_param/ShaderParam.hh b/src/systems/shader_param/ShaderParam.hh index 6c6d4a4215..8197d08015 100644 --- a/src/systems/shader_param/ShaderParam.hh +++ b/src/systems/shader_param/ShaderParam.hh @@ -35,42 +35,42 @@ namespace systems /// \brief A plugin for setting shaders to a visual and its params /// - /// Plugin parameters: + /// ## System Parameters /// - /// Shader to use - can be repeated to specify shader programs - /// written in different languages. - /// Attributes: - /// language - Shader language. Possible values: glsl, metal - /// Default to glsl if not specified - /// Path to vertex program - /// Path to fragment program - /// Shader parameter - can be repeated within plugin SDF element - /// Name of uniform variable bound to the shader - /// Type of shader, i.e. vertex, fragment - /// Variable type: float, int, float_array, int_array - /// Value to set the shader parameter to. The vallue string can - /// be an int, float, or a space delimited array of ints or - /// floats. It can also be 'TIME', in which case the value will - /// be bound to sim time. + /// - ``: Shader to use - can be repeated to specify shader programs + /// written in different languages. + /// - Attributes: + /// - `language`: Shader language. Possible values: glsl, metal + /// Default to glsl if not specified + /// - ``: Path to vertex program + /// - ``: Path to fragment program + /// - ``: Shader parameter - can be repeated within plugin SDF element + /// - ``: Name of uniform variable bound to the shader + /// - ``: Type of shader, i.e. vertex, fragment + /// - ``: Variable type: float, int, float_array, int_array + /// - ``: Value to set the shader parameter to. The vallue string can + /// be an int, float, or a space delimited array of ints or + /// floats. It can also be "TIME", in which case the value will + /// be bound to sim time. /// - /// Example usage: + /// ## Example /// - /// \verbatim - /// - /// - /// materials/my_vs.glsl - /// materials/my_fs.glsl - /// - /// - /// - /// ambient - /// fragment - /// float_array - /// 1.0 0.0 0.0 1.0 - /// - /// - /// \endverbatim + /// ``` + /// + /// + /// materials/my_vs.glsl + /// materials/my_fs.glsl + /// + /// + /// + /// ambient + /// fragment + /// float_array + /// 1.0 0.0 0.0 1.0 + /// + /// + /// ``` class ShaderParam : public System, public ISystemConfigure, diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 5c9cdab562..708fddf883 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -46,64 +46,75 @@ namespace systems /// `/model/{ns}/joint/{joint_name}/force`. /// /// ## System Parameters - /// - - The namespace in which the robot exists. The plugin will + /// + /// - ``: The namespace in which the robot exists. The plugin will /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`or /// `/model/{namespace}/joint/{joint_name}/cmd_vel` depending on the mode of /// operation. If {topic} is set then the plugin will listen on /// {namespace}/{topic} /// [Optional] - /// - - The topic for receiving thrust commands. [Optional] - /// - - This is the joint in the model which corresponds to the + /// - ``: The topic for receiving thrust commands. [Optional] + /// - ``: This is the joint in the model which corresponds to the /// propeller. [Required] - /// - - If set to true will make the thruster + /// - ``: If set to true will make the thruster /// plugin accept commands in angular velocity in radians per seconds in /// terms of newtons. [Optional, Boolean, defaults to false] - /// - - The fluid density of the liquid in which the thruster + /// - ``: The fluid density of the liquid in which the thruster /// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3] - /// - - The diameter of the propeller in meters. + /// - ``: The diameter of the propeller in meters. /// [Optional, m, defaults to 0.02m] - /// - - This is the coefficient which relates the angular - /// velocity to thrust. A positive coefficient corresponds to a clockwise - /// propeller, which is a propeller that spins clockwise under positive - /// thrust when viewed along the parent link from stern (-x) to bow (+x). - /// [Optional, no units, defaults to 1.0] - /// - /// omega = sqrt(thrust / - /// (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) - /// - /// Where omega is the propeller's angular velocity in rad/s. - /// - - If true, use joint velocity commands to rotate the + /// - ``: This is the coefficient which relates the + /// angular velocity to thrust. A positive coefficient corresponds to a + /// clockwise propeller, which is a propeller that spins clockwise under + /// positive thrust when viewed along the parent link from stern (-x) to + /// bow (+x). [Optional, no units, defaults to 1.0] + /// ``` + /// omega = sqrt(thrust / + /// (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + /// ``` + /// where omega is the propeller's angular velocity in rad/s. + /// - ``: If true, use joint velocity commands to rotate the /// propeller. If false, use a PID controller to apply wrenches directly to /// the propeller link instead. [Optional, defaults to false]. - /// - - Proportional gain for joint PID controller. [Optional, - /// no units, defaults to 0.1] - /// - - Integral gain for joint PID controller. [Optional, - /// no units, defaults to 0.0] - /// - - Derivative gain for joint PID controller. [Optional, - /// no units, defaults to 0.0] - /// - - Maximum input thrust or angular velocity command. - /// [Optional, defaults to 1000N or 1000rad/s] - /// - - Minimum input thrust or angular velocity command. - /// [Optional, defaults to -1000N or -1000rad/s] - /// - - Deadband of the thruster. Absolute value below which the - /// thruster won't spin nor generate thrust. This value can - /// be changed at runtime using a topic. The topic is either - /// `/model/{ns}/joint/{jointName}/enable_deadband` or - /// `{ns}/{topic}/enable_deadband` depending on other params - /// - - Relative speed reduction between the water + /// - ``: Proportional gain for joint PID controller. [Optional, + /// no units, defaults to 0.1] + /// - ``: Integral gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// - ``: Derivative gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// - ``: Maximum input thrust or angular velocity command. + /// [Optional, defaults to 1000N or 1000rad/s] + /// - ``: Minimum input thrust or angular velocity command. + /// [Optional, defaults to -1000N or -1000rad/s] + /// - ``: Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust. This value can + /// be changed at runtime using a topic. The topic is either + /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `{ns}/{topic}/enable_deadband` depending on other params + /// - ``: Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] - /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: - /// Va = (1 - wake_fraction) * advance_speed - /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coefficient (Kt). - /// [Optional, defults to 1] - /// - - Constant given by the open water propeller diagram. Used - /// in the calculation of the thrust coefficient (Kt). - /// [Optional, defults to 0] - /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: - /// Kt = alpha_1 * alpha_2 * (Va/(propeller_revolution * propeller_diameter)) + /// + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// ``` + /// Va = (1 - wake_fraction) * advance_speed + /// ``` + /// + /// - ``: Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 1] + /// - ``: Constant given by the open water propeller diagram. Used + /// in the calculation of the thrust coefficient (Kt). + /// [Optional, defults to 0] + /// + /// See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 95: + /// ``` + /// Kt = alpha_1 * alpha_2 * + /// (Va / (propeller_revolution * propeller_diameter)) + /// ``` + /// /// ## Example + /// /// An example configuration is installed with Gazebo. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the /// thruster plugin to propell the craft and the buoyancy plugin for buoyant @@ -112,15 +123,17 @@ namespace systems /// gz sim auv_controls.sdf /// ``` /// To control the rudder of the craft run the following: - /// ``` - /// gz topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos - /// -m gz.msgs.Double -p 'data: -0.17' - /// ``` + /** ``` + gz topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos \ + -m gz.msgs.Double -p 'data: -0.17' + ``` + **/ /// To apply a thrust you may run the following command: - /// ``` - /// gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust - /// -m gz.msgs.Double -p 'data: -31' - /// ``` + /** ``` + gz topic -t /model/tethys/joint/propeller_joint/cmd_thrust \ + -m gz.msgs.Double -p 'data: -31' + ``` + **/ /// The vehicle should move in a circle. class Thruster: public gz::sim::System, diff --git a/src/systems/touch_plugin/TouchPlugin.hh b/src/systems/touch_plugin/TouchPlugin.hh index 0912671ca0..da1be1e841 100644 --- a/src/systems/touch_plugin/TouchPlugin.hh +++ b/src/systems/touch_plugin/TouchPlugin.hh @@ -42,7 +42,7 @@ namespace systems /// The plugin requires that a contact sensors is placed in at least one /// link on the model on which this plugin is attached. /// - /// Parameters: + /// ## System Parameters /// /// - `` Name, or substring of a name, that identifies the target /// collision entity/entities. @@ -50,15 +50,15 @@ namespace systems /// entities, so it can possibly match more than one collision. /// For example, using the name of a model will match all of its /// collisions (scoped name - /// /model_name/link_name/collision_name). + /// `/model_name/link_name/collision_name`). /// /// - `