diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index cf70ca6852..705db4f35d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -14,13 +14,12 @@ jobs: uses: ignition-tooling/action-ignition-ci@bionic with: codecov-token: ${{ secrets.CODECOV_TOKEN }} - # TODO(anyone) Enable Focal CI and fix failing tests - # focal-ci: - # runs-on: ubuntu-latest - # name: Ubuntu Focal CI - # steps: - # - name: Checkout - # uses: actions/checkout@v2 - # - name: Compile and test - # id: ci - # uses: ignition-tooling/action-ignition-ci@focal + focal-ci: + runs-on: ubuntu-latest + name: Ubuntu Focal CI + steps: + - name: Checkout + uses: actions/checkout@v2 + - name: Compile and test + id: ci + uses: ignition-tooling/action-ignition-ci@focal diff --git a/CMakeLists.txt b/CMakeLists.txt index c80396d494..c662f6ac03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project @@ -38,7 +38,7 @@ endif() # Search for project-specific dependencies #============================================================================ -ign_find_package(sdformat11 REQUIRED) +ign_find_package(sdformat11 REQUIRED VERSION 11.1) set(SDF_VER ${sdformat11_VERSION_MAJOR}) #-------------------------------------- @@ -53,7 +53,7 @@ set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs7 REQUIRED) +ign_find_package(ignition-msgs7 REQUIRED VERSION 7.1) set(IGN_MSGS_VER ${ignition-msgs7_VERSION_MAJOR}) #-------------------------------------- diff --git a/Changelog.md b/Changelog.md index 2629c4db0d..6df3c1c169 100644 --- a/Changelog.md +++ b/Changelog.md @@ -135,6 +135,78 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.x.x (202x-xx-xx) + +### Ignition Gazebo 4.8.0 (2021-04-22) + +1. Add odometry publisher system. + * [Pull Request 547](https://github.com/ignitionrobotics/ign-gazebo/pull/547) + +1. Patch particle emitter2 service. + * [Pull Request 777](https://github.com/ignitionrobotics/ign-gazebo/pull/777) + +### Ignition Gazebo 4.7.0 (2021-04-09) + +1. Particle emitter based on SDF. + * [Pull Request 730](https://github.com/ignitionrobotics/ign-gazebo/pull/730) + +1. Fix log playback for particle emitters. + * [Pull Request 745](https://github.com/ignitionrobotics/ign-gazebo/pull/745) + +1. ECM's ChangedState gets message with modified components. + * [Pull Request 742](https://github.com/ignitionrobotics/ign-gazebo/pull/742) + +1. Fixed collision visual bounding boxes. + * [Pull Request 746](https://github.com/ignitionrobotics/ign-gazebo/pull/746) + +1. Fix compute_rtfs arguments. + * [Pull Request 737](https://github.com/ignitionrobotics/ign-gazebo/pull/737) + +1. Validate step size and RTF parameters. + * [Pull Request 740](https://github.com/ignitionrobotics/ign-gazebo/pull/740) + +1. Use Protobuf_IMPORT_DIRS instead of PROTOBUF_IMPORT_DIRS for + compatibility with Protobuf CMake config. + * [Pull Request 715](https://github.com/ignitionrobotics/ign-gazebo/pull/715) + +1. Do not pass -Wno-unused-parameter to MSVC compiler. + * [Pull Request 716](https://github.com/ignitionrobotics/ign-gazebo/pull/716) + +1. Support configuring particle scatter ratio in particle emitter system. + * [Pull Request 674](https://github.com/ignitionrobotics/ign-gazebo/pull/674) + +1. Fix diffuse and ambient values for ackermann example. + * [Pull Request 707](https://github.com/ignitionrobotics/ign-gazebo/pull/707) + +1. Scenebroadcaster sensors. + * [Pull Request 698](https://github.com/ignitionrobotics/ign-gazebo/pull/698) + +1. Add thermal camera test for object temperature below 0. + * [Pull Request 621](https://github.com/ignitionrobotics/ign-gazebo/pull/621) + +1. [BULLET] Making GetContactsFromLastStepFeature optional in Collision Features + * [Pull Request 690](https://github.com/ignitionrobotics/ign-gazebo/pull/690) + +1. Fix joint controller GUI test. + * [Pull Request 697](https://github.com/ignitionrobotics/ign-gazebo/pull/697) + +1. Quiet warnings from Joint State Publisher. + * [Pull Request 696](https://github.com/ignitionrobotics/ign-gazebo/pull/696) + +1. Ackermann Steering Plugin. + * [Pull Request 618](https://github.com/ignitionrobotics/ign-gazebo/pull/618) + +1. Remove bounding box when model is deleted + * [Pull Request 675](https://github.com/ignitionrobotics/ign-gazebo/pull/675) + +1. Cache link poses to improve performance. + * [Pull Request 669](https://github.com/ignitionrobotics/ign-gazebo/pull/669) + +1. Check empty world name in Scene3d. + * [Pull Request 662](https://github.com/ignitionrobotics/ign-gazebo/pull/662) + +1. All changes up to 3.8.0. + ### Ignition Gazebo 4.6.0 (2021-03-01) 1. Use a custom data structure to manage entity feature maps. @@ -460,6 +532,115 @@ ## Ignition Gazebo 3.x +### Ignition Gazebo 3.X.X (202X-XX-XX) + +### Ignition Gazebo 3.8.0 (2021-03-17) + +1. Add joint position controller GUI, also enable tests for GUI plugins + * [Pull request #534](https://github.com/ignitionrobotics/ign-gazebo/pull/534) + +1. Remove visibility from headers that are not installed + * [Pull request #665](https://github.com/ignitionrobotics/ign-gazebo/pull/665) + +1. Added screenshot to toolbar + * [Pull request #588](https://github.com/ignitionrobotics/ign-gazebo/pull/588) + +1. Improve ign tool support on macOS + * [Pull request #477](https://github.com/ignitionrobotics/ign-gazebo/pull/477) + +1. change nullptr to a int ptr for qt 5.15.2 bug + * [Pull request #527](https://github.com/ignitionrobotics/ign-gazebo/pull/527) + +1. Kinetic energy monitor plugin + * [Pull request #492](https://github.com/ignitionrobotics/ign-gazebo/pull/492) + +1. Use a std::promise/std::future to avoid busy waiting the step ack messages in NetworkManagerPrimary + * [Pull request #470](https://github.com/ignitionrobotics/ign-gazebo/pull/470) + +1. clarified performer example + * [Pull request #390](https://github.com/ignitionrobotics/ign-gazebo/pull/390) + +1. Add tutorial tweaks + * [Pull request #380](https://github.com/ignitionrobotics/ign-gazebo/pull/380) + +1. Fix Qt5 warnings for using anchors + * [Pull request #363](https://github.com/ignitionrobotics/ign-gazebo/pull/363) + +1. Update codeowners + * [Pull request #305](https://github.com/ignitionrobotics/ign-gazebo/pull/305) + +1. Qt auto scale factor for HiDPI displays + * [Pull request #291](https://github.com/ignitionrobotics/ign-gazebo/pull/291) + +1. Fix yaw units + * [Pull request #238](https://github.com/ignitionrobotics/ign-gazebo/pull/238) + +1. Fixed docblock showGrid + * [Pull request #152](https://github.com/ignitionrobotics/ign-gazebo/pull/152) + +1. Fix entity tree for large worlds + * [Pull request #673](https://github.com/ignitionrobotics/ign-gazebo/pull/673) + +1. Master branch updates + * [Pull request #672](https://github.com/ignitionrobotics/ign-gazebo/pull/672) + +1. Backport #561: Use common::setenv + * [Pull request #666](https://github.com/ignitionrobotics/ign-gazebo/pull/666) + +1. Use a custom data structure to manage entity feature maps + * [Pull request #586](https://github.com/ignitionrobotics/ign-gazebo/pull/586) + +1. Limit scene broadcast publications when paused + * [Pull request #497](https://github.com/ignitionrobotics/ign-gazebo/pull/497) + +1. Fix flaky SceneBoradcaster test + * [Pull request #641](https://github.com/ignitionrobotics/ign-gazebo/pull/641) + +1. Add TF/Pose_V publisher in DiffDrive + * [Pull request #548](https://github.com/ignitionrobotics/ign-gazebo/pull/548) + +1. 👩‍🌾 Relax performance test + * [Pull request #640](https://github.com/ignitionrobotics/ign-gazebo/pull/640) + +1. 👩‍🌾 Improve velocity control test + * [Pull request #642](https://github.com/ignitionrobotics/ign-gazebo/pull/642) + +1. Add `laser_retro` support + * [Pull request #603](https://github.com/ignitionrobotics/ign-gazebo/pull/603) + +1. Fix pose of plane visual with non-default normal vector + * [Pull request #574](https://github.com/ignitionrobotics/ign-gazebo/pull/574) + +1. Add About dialog + * [Pull request #609](https://github.com/ignitionrobotics/ign-gazebo/pull/609) + +1. Make topics configurable for joint controllers + * [Pull request #584](https://github.com/ignitionrobotics/ign-gazebo/pull/584) + +1. Also use Ignition GUI render event + * [Pull request #598](https://github.com/ignitionrobotics/ign-gazebo/pull/598) + +1. Tutorial on migrating SDF files from Gazebo classic + * [Pull request #400](https://github.com/ignitionrobotics/ign-gazebo/pull/400) + +1. Visualize collisions + * [Pull request #531](https://github.com/ignitionrobotics/ign-gazebo/pull/531) + +1. Backport state update changes from pull request #486 + * [Pull request #583](https://github.com/ignitionrobotics/ign-gazebo/pull/583) + +1. Publish all periodic change components in Scene Broadcaster + * [Pull request #544](https://github.com/ignitionrobotics/ign-gazebo/pull/544) + +1. added size to `ground_plane` in examples + * [Pull request #573](https://github.com/ignitionrobotics/ign-gazebo/pull/573) + +1. Parallelize State call in ECM + * [Pull request #451](https://github.com/ignitionrobotics/ign-gazebo/pull/451) + +1. Non-blocking paths request + * [Pull request #555](https://github.com/ignitionrobotics/ign-gazebo/pull/555) + ### Ignition Gazebo 3.7.0 (2021-01-13) 1. Fix examples in migration plugins tutorial. diff --git a/examples/worlds/particle_emitter.sdf b/examples/worlds/particle_emitter.sdf index 58fb36c3be..bf372aeef2 100644 --- a/examples/worlds/particle_emitter.sdf +++ b/examples/worlds/particle_emitter.sdf @@ -1,5 +1,10 @@ + + + + + + + + 0.001 + 1.0 + + + + libignition-physics-tpe-plugin.so + + + + + + + + + + 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 + + + + + + + 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 + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/fog generator + + + + + true + 20 20 0 0 0 0 + + + + true + 10 10 0 + 1 1 1 + 25 + 0.1 + 0.2 + 0.5 + 5 + + 1.0 0.0 0.0 + 1.0 0.0 0.0 + + + + + + + + diff --git a/examples/worlds/pendulum_links.sdf b/examples/worlds/pendulum_links.sdf new file mode 100644 index 0000000000..2f62bab25c --- /dev/null +++ b/examples/worlds/pendulum_links.sdf @@ -0,0 +1,279 @@ + + + + + + + + 0.001 + 1.0 + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + + 100 + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.01 0 0 0 + + + 0.8 + 0.02 + + + + + -0.275 0 1.1 0 0 0 + + + 0.2 0.2 2.2 + + + + + + + 0 0 2.1 -1.5708 0 0 + 0 + + 0 0 0.5 0 0 0 + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + -0.05 0 0 0 1.5708 0 + + + 0.1 + 0.3 + + + + + 0 0 1.0 0 1.5708 0 + + + 0.1 + 0.2 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + 0.25 1.0 2.1 -2 0 0 + 0 + + 0 0 0.5 0 0 0 + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0 0 0 0 1.5708 0 + + + 0.08 + 0.3 + + + + + 0 0 0.5 0 0 0 + + + 0.1 + 0.9 + + + + + + + base + upper_link + + 1.0 0 0 + + + + + upper_link + lower_link + + 1.0 0 0 + + + + + base + upper_link + lower_link + + + + diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index 53d3b025ac..827abab211 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -47,6 +48,7 @@ #include #include #include +#include #include #include #include @@ -639,6 +641,42 @@ namespace ignition /// \return Axis aligned box object. template<> math::AxisAlignedBox convert(const msgs::AxisAlignedBox &_in); + + /// \brief Generic conversion from a particle emitter SDF object to another + /// type. + /// \param[in] _in Particle emitter SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::ParticleEmitter &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a particle emitter SDF object to + /// a particle emitter message object. + /// \param[in] _in Particle emitter SDF object. + /// \return Particle emitter message. + template<> + msgs::ParticleEmitter convert(const sdf::ParticleEmitter &_in); + + /// \brief Generic conversion from a particle emitter SDF object to another + /// type. + /// \param[in] _in Particle emitter SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::ParticleEmitter &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a particle emitter SDF object to + /// a particle emitter message object. + /// \param[in] _in Particle emitter SDF object. + /// \return Particle emitter message. + template<> + sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); } } } diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index e2098d5fa1..707c52ce2d 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -475,11 +475,9 @@ namespace ignition /// \brief Get a message with the serialized state of all entities and /// components that are changing in the current iteration /// - /// Currently supported: + /// This includes: /// * New entities and all of their components /// * Removed entities and all of their components - /// - /// Future work: /// * Entities which had a component added /// * Entities which had a component removed /// * Entities which had a component modified @@ -536,11 +534,9 @@ namespace ignition /// \brief Get a message with the serialized state of all entities and /// components that are changing in the current iteration /// - /// Currently supported: + /// This includes: /// * New entities and all of their components /// * Removed entities and all of their components - /// - /// Future work: /// * Entities which had a component added /// * Entities which had a component removed /// * Entities which had a component modified diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index a3590c1da9..e5c2442279 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -140,6 +141,13 @@ namespace ignition /// \sa CreateEntities(const sdf::Model *) public: Entity CreateEntities(const sdf::Sensor *_sensor); + /// \brief Create all entities that exist in the + /// sdf::ParticleEmitter object. + /// \param[in] _emitter SDF ParticleEmitter object. + /// \return ParticleEmitter entity. + /// \sa CreateEntities(const sdf::Link *) + public: Entity CreateEntities(const sdf::ParticleEmitter *_emitter); + /// \brief Request an entity deletion. This will insert the request /// into a queue. The queue is processed toward the end of a simulation /// update step. diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index cfa49e98d3..83c5ba7bef 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -155,6 +155,26 @@ namespace ignition std::string IGNITION_GAZEBO_VISIBLE validTopic( const std::vector &_topics); + /// \brief Helper function that returns a valid Ignition Transport topic + /// consisting of the scoped name for the provided entity. + /// + /// For example, if the provided entity has a scoped name of + /// `my_model::my_link::my_sensor` then the resulting topic name will + /// be `/model/my_model/link/my_link/sensor/my_sensor`. If _excludeWorld + /// is false, then the topic name will be prefixed by `/world/WORLD_NAME/`, + /// where `WORLD_NAME` is the name of the world. + /// + /// \param[in] _entity The entity to generate the topic name for. + /// \param[in] _ecm The entity component manager. + /// \param[in] _excludeWorld True to exclude the world name from the topic. + /// \return An Ignition Transport topic name based on the scoped name of + /// the provided entity, or empty string if a topic name could not be + /// generated. + std::string IGNITION_GAZEBO_VISIBLE topicFromScopedName( + const Entity &_entity, + const EntityComponentManager &_ecm, + bool _excludeWorld = true); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; @@ -168,6 +188,7 @@ namespace ignition /// \brief Environment variable holding paths to custom rendering engine /// plugins. const std::string kRenderPluginPathEnv{"IGN_GAZEBO_RENDER_ENGINE_PATH"}; + } } } diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index c83a3d1a67..9023a779ef 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,10 +10,12 @@ set_source_files_properties( ) # Suppress compiler warnings in generated protobuf C++ code. -set_source_files_properties( - ${PROTO_PRIVATE_SRC} - COMPILE_FLAGS -Wno-unused-parameter -) +if(NOT MSVC) + set_source_files_properties( + ${PROTO_PRIVATE_SRC} + COMPILE_FLAGS -Wno-unused-parameter + ) +endif() set(network_sources network/NetworkConfig.cc diff --git a/src/Conversions.cc b/src/Conversions.cc index 2adb6efe69..322b912221 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1496,3 +1496,130 @@ math::AxisAlignedBox ignition::gazebo::convert(const msgs::AxisAlignedBox &_in) out.Max() = msgs::Convert(_in.max_corner()); return out; } + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::ParticleEmitter ignition::gazebo::convert(const sdf::ParticleEmitter &_in) +{ + msgs::ParticleEmitter out; + out.set_name(_in.Name()); + switch(_in.Type()) + { + default: + case sdf::ParticleEmitterType::POINT: + out.set_type(msgs::ParticleEmitter::POINT); + break; + case sdf::ParticleEmitterType::BOX: + out.set_type(msgs::ParticleEmitter::BOX); + break; + case sdf::ParticleEmitterType::CYLINDER: + out.set_type(msgs::ParticleEmitter::CYLINDER); + break; + case sdf::ParticleEmitterType::ELLIPSOID: + out.set_type(msgs::ParticleEmitter::ELLIPSOID); + break; + } + + msgs::Set(out.mutable_pose(), _in.RawPose()); + msgs::Set(out.mutable_size(), _in.Size()); + msgs::Set(out.mutable_particle_size(), _in.ParticleSize()); + out.mutable_rate()->set_data(_in.Rate()); + out.mutable_duration()->set_data(_in.Duration()); + out.mutable_emitting()->set_data(_in.Emitting()); + out.mutable_lifetime()->set_data(_in.Lifetime()); + if (_in.Material()) + { + out.mutable_material()->CopyFrom(convert(*_in.Material())); + } + out.mutable_min_velocity()->set_data(_in.MinVelocity()); + out.mutable_max_velocity()->set_data(_in.MaxVelocity()); + msgs::Set(out.mutable_color_start(), _in.ColorStart()); + msgs::Set(out.mutable_color_end(), _in.ColorEnd()); + out.mutable_scale_rate()->set_data(_in.ScaleRate()); + out.mutable_color_range_image()->set_data(_in.ColorRangeImage()); + + if (!_in.ColorRangeImage().empty()) + { + std::string path = asFullPath(_in.ColorRangeImage(), _in.FilePath()); + + common::SystemPaths systemPaths; + systemPaths.SetFilePathEnv(kResourcePathEnv); + std::string absolutePath = systemPaths.FindFile(path); + + if (!absolutePath.empty()) + out.mutable_color_range_image()->set_data(absolutePath); + } + + /// \todo(nkoenig) Modify the particle_emitter.proto file to + /// have a topic field. + if (!_in.Topic().empty()) + { + auto header = out.mutable_header()->add_data(); + header->set_key("topic"); + header->add_value(_in.Topic()); + } + + return out; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) +{ + sdf::ParticleEmitter out; + out.SetName(_in.name()); + switch(_in.type()) + { + default: + case msgs::ParticleEmitter::POINT: + out.SetType(sdf::ParticleEmitterType::POINT); + break; + case msgs::ParticleEmitter::BOX: + out.SetType(sdf::ParticleEmitterType::BOX); + break; + case msgs::ParticleEmitter::CYLINDER: + out.SetType(sdf::ParticleEmitterType::CYLINDER); + break; + case msgs::ParticleEmitter::ELLIPSOID: + out.SetType(sdf::ParticleEmitterType::ELLIPSOID); + break; + } + out.SetRawPose(msgs::Convert(_in.pose())); + out.SetSize(msgs::Convert(_in.size())); + out.SetParticleSize(msgs::Convert(_in.particle_size())); + out.SetMinVelocity(msgs::Convert(_in.min_velocity())); + out.SetMaxVelocity(msgs::Convert(_in.max_velocity())); + out.SetColorStart(msgs::Convert(_in.color_start())); + out.SetColorEnd(msgs::Convert(_in.color_end())); + + if (_in.has_material()) + { + out.SetMaterial(convert(_in.material())); + } + + if (_in.has_rate()) + out.SetRate(_in.rate().data()); + if (_in.has_duration()) + out.SetDuration(_in.duration().data()); + if (_in.has_emitting()) + out.SetEmitting(_in.emitting().data()); + if (_in.has_lifetime()) + out.SetLifetime(_in.lifetime().data()); + if (_in.has_scale_rate()) + out.SetScaleRate(_in.scale_rate().data()); + if (_in.has_color_range_image()) + out.SetColorRangeImage(_in.color_range_image().data()); + + for (int i = 0; i < _in.header().data_size(); ++i) + { + auto data = _in.header().data(i); + if (data.key() == "topic" && data.value_size() > 0) + { + out.SetTopic(data.value(0)); + } + } + + return out; +} diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 9d3a1ba5f2..e957851bda 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -941,3 +941,83 @@ TEST(Conversions, Actor) EXPECT_EQ(math::Pose3d(6, 5, 4, 0, 0, 0), newActor.TrajectoryByIndex(0)->WaypointByIndex(0)->Pose()); } + +///////////////////////////////////////////////// +TEST(Conversions, ParticleEmitter) +{ + sdf::ParticleEmitter emitter; + emitter.SetName("my_emitter"); + emitter.SetType(sdf::ParticleEmitterType::BOX); + emitter.SetEmitting(false); + emitter.SetDuration(12); + emitter.SetLifetime(56); + emitter.SetRate(0.5); + emitter.SetScaleRate(1.2); + emitter.SetMinVelocity(0.1); + emitter.SetMaxVelocity(0.2); + emitter.SetSize(math::Vector3d(1, 2, 3)); + emitter.SetParticleSize(math::Vector3d(4, 5, 6)); + emitter.SetColorStart(math::Color(0.1, 0.2, 0.3)); + emitter.SetColorEnd(math::Color(0.4, 0.5, 0.6)); + emitter.SetColorRangeImage("range_image"); + emitter.SetTopic("my_topic"); + emitter.SetRawPose(math::Pose3d(1, 2, 3, 0, 0, 0)); + + sdf::Material material; + sdf::Pbr pbr; + sdf::PbrWorkflow workflow; + workflow.SetType(sdf::PbrWorkflowType::METAL); + workflow.SetAlbedoMap("albedo_map.png"); + pbr.SetWorkflow(workflow.Type(), workflow); + material.SetPbrMaterial(pbr); + + emitter.SetMaterial(material); + + // Convert SDF to a message. + msgs::ParticleEmitter emitterMsg = convert(emitter); + + EXPECT_EQ("my_emitter", emitterMsg.name()); + EXPECT_EQ(msgs::ParticleEmitter::BOX, emitterMsg.type()); + EXPECT_FALSE(emitterMsg.emitting().data()); + EXPECT_NEAR(12, emitterMsg.duration().data(), 1e-3); + EXPECT_NEAR(56, emitterMsg.lifetime().data(), 1e-3); + EXPECT_NEAR(0.5, emitterMsg.rate().data(), 1e-3); + EXPECT_NEAR(1.2, emitterMsg.scale_rate().data(), 1e-3); + EXPECT_NEAR(0.1, emitterMsg.min_velocity().data(), 1e-3); + EXPECT_NEAR(0.2, emitterMsg.max_velocity().data(), 1e-3); + EXPECT_EQ(math::Vector3d(1, 2, 3), msgs::Convert(emitterMsg.size())); + EXPECT_EQ(math::Vector3d(4, 5, 6), msgs::Convert(emitterMsg.particle_size())); + EXPECT_EQ(math::Color(0.1, 0.2, 0.3), + msgs::Convert(emitterMsg.color_start())); + EXPECT_EQ(math::Color(0.4, 0.5, 0.6), msgs::Convert(emitterMsg.color_end())); + EXPECT_EQ("range_image", emitterMsg.color_range_image().data()); + + auto header = emitterMsg.header().data(0); + EXPECT_EQ("topic", header.key()); + EXPECT_EQ("my_topic", header.value(0)); + + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), msgs::Convert(emitterMsg.pose())); + + auto pbrMsg = emitterMsg.material().pbr(); + EXPECT_EQ(msgs::Material::PBR::METAL, pbrMsg.type()); + EXPECT_EQ("albedo_map.png", pbrMsg.albedo_map()); + + // Convert the message back to SDF. + sdf::ParticleEmitter emitter2 = convert(emitterMsg); + EXPECT_EQ(emitter2.Name(), emitter.Name()); + EXPECT_EQ(emitter2.Type(), emitter.Type()); + EXPECT_EQ(emitter2.Emitting(), emitter.Emitting()); + EXPECT_NEAR(emitter2.Duration(), emitter.Duration(), 1e-3); + EXPECT_NEAR(emitter2.Lifetime(), emitter.Lifetime(), 1e-3); + EXPECT_NEAR(emitter2.Rate(), emitter.Rate(), 1e-3); + EXPECT_NEAR(emitter2.ScaleRate(), emitter.ScaleRate(), 1e-3); + EXPECT_NEAR(emitter2.MinVelocity(), emitter.MinVelocity(), 1e-3); + EXPECT_NEAR(emitter2.MaxVelocity(), emitter.MaxVelocity(), 1e-3); + EXPECT_EQ(emitter2.Size(), emitter.Size()); + EXPECT_EQ(emitter2.ParticleSize(), emitter.ParticleSize()); + EXPECT_EQ(emitter2.ColorStart(), emitter.ColorStart()); + EXPECT_EQ(emitter2.ColorEnd(), emitter.ColorEnd()); + EXPECT_EQ(emitter2.ColorRangeImage(), emitter.ColorRangeImage()); + EXPECT_EQ(emitter2.Topic(), emitter.Topic()); + EXPECT_EQ(emitter2.RawPose(), emitter.RawPose()); +} diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 11047a896c..72299ac516 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -72,6 +72,12 @@ class ignition::gazebo::EntityComponentManagerPrivate msgs::SerializedStateMap &_msg, const std::unordered_set &_types = {}); + /// \brief Add newly modified (created/modified/removed) components to + /// modifiedComponents list. The entity is added to the list when it is not + /// a newly created entity or is not an entity to be removed + /// \param[in] _entity Entity that has component newly modified + public: void AddModifiedComponent(const Entity &_entity); + /// \brief Map of component storage classes. The key is a component /// type id, and the value is a pointer to the component storage. public: std::unordered_map toRemoveEntities; + /// \brief Entities that have components newly modified + /// (created/modified/removed) but are not entities that have been + /// newly created or removed (ie. newlyCreatedEntities or toRemoveEntities). + /// This is used for the ChangedState functions + public: std::unordered_set modifiedComponents; + /// \brief Flag that indicates if all entities should be removed. public: bool removeAllEntities{false}; @@ -105,7 +117,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// NOTE: Any modification of this data structure must be followed /// by setting `entityComponentsDirty` to true. public: std::unordered_map> entityComponents; + std::unordered_map> entityComponents; /// \brief A vector of iterators to evenly distributed spots in the /// `entityComponents` map. Threads in the `State` function use this @@ -113,7 +125,7 @@ class ignition::gazebo::EntityComponentManagerPrivate /// is recalculated if `entityComponents` is changed (when /// `entityComponentsDirty` == true). public: std::vector>::iterator> + std::unordered_map>::iterator> entityComponentIterators; /// \brief A mutex to protect newly created entities. @@ -305,8 +317,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests() { for (const auto &key : entityIter->second) { - this->dataPtr->components.at(key.second.first)->Remove( - key.second.second); + this->dataPtr->components.at(key.first)->Remove(key.second); } // Remove the entry in the entityComponent map @@ -354,6 +365,8 @@ bool EntityComponentManager::RemoveComponent( this->UpdateViews(_entity); + this->dataPtr->AddModifiedComponent(_entity); + // Add component to map of removed components { std::lock_guard lock(this->dataPtr->removedComponentsMutex); @@ -424,12 +437,14 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity, if (typeKey == ecIter->second.end()) return result; - if (this->dataPtr->oneTimeChangedComponents.find(typeKey->second) != + ComponentKey key{_typeId, typeKey->second}; + + if (this->dataPtr->oneTimeChangedComponents.find(key) != this->dataPtr->oneTimeChangedComponents.end()) { result = ComponentState::OneTimeChange; } - else if (this->dataPtr->periodicChangedComponents.find(typeKey->second) != + else if (this->dataPtr->periodicChangedComponents.find(key) != this->dataPtr->periodicChangedComponents.end()) { result = ComponentState::PeriodicChange; @@ -529,6 +544,8 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( } } + this->dataPtr->AddModifiedComponent(_entity); + // Instantiate the new component. std::pair componentIdPair = this->dataPtr->components[_componentTypeId]->Create(_data); @@ -536,7 +553,7 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( ComponentKey componentKey{_componentTypeId, componentIdPair.first}; this->dataPtr->entityComponents[_entity].insert( - {_componentTypeId, componentKey}); + {_componentTypeId, componentIdPair.first}); this->dataPtr->oneTimeChangedComponents.insert(componentKey); this->dataPtr->entityComponentsDirty = true; @@ -581,7 +598,7 @@ ComponentId EntityComponentManager::EntityComponentIdFromType( auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return typeIter->second.second; + return typeIter->second; return -1; } @@ -599,8 +616,8 @@ const components::BaseComponent auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(typeIter->second.first)->Component( - typeIter->second.second); + return this->dataPtr->components.at(_type)->Component( + typeIter->second); return nullptr; } @@ -616,8 +633,7 @@ components::BaseComponent *EntityComponentManager::ComponentImplementation( auto typeIter = ecIter->second.find(_type); if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(typeIter->second.first)->Component( - typeIter->second.second); + return this->dataPtr->components.at(_type)->Component(typeIter->second); return nullptr; } @@ -879,16 +895,14 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, for (const ComponentTypeId type : types) { // If the entity does not have the component, continue - std::unordered_map::iterator typeIter = - iter->second.find(type); + auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) { continue; } - ComponentKey comp = (typeIter->second); auto compMsg = entityMsg->add_components(); - auto compBase = this->ComponentImplementation(_entity, comp.first); + auto compBase = this->ComponentImplementation(_entity, type); compMsg->set_type(compBase->TypeId()); std::ostringstream ostr; @@ -946,16 +960,16 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, // Empty means all types for (const ComponentTypeId type : types) { - std::unordered_map::iterator typeIter = - iter->second.find(type); + auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) { continue; } - ComponentKey comp = typeIter->second; const components::BaseComponent *compBase = - this->ComponentImplementation(_entity, comp.first); + this->ComponentImplementation(_entity, type); + + ComponentKey comp = {type, typeIter->second}; // If not sending full state, skip unchanged components if (!_full && @@ -1021,7 +1035,11 @@ ignition::msgs::SerializedState EntityComponentManager::ChangedState() const this->AddEntityToMessage(stateMsg, entity); } - // TODO(anyone) New / removed / changed components + // New / removed / changed components + for (const auto &entity : this->dataPtr->modifiedComponents) + { + this->AddEntityToMessage(stateMsg, entity); + } return stateMsg; } @@ -1042,7 +1060,11 @@ void EntityComponentManager::ChangedState( this->AddEntityToMessage(_state, entity); } - // TODO(anyone) New / removed / changed components + // New / removed / changed components + for (const auto &entity : this->dataPtr->modifiedComponents) + { + this->AddEntityToMessage(_state, entity); + } } ////////////////////////////////////////////////// @@ -1257,6 +1279,10 @@ void EntityComponentManager::SetState( // values. // igndbg << *comp << " " << *newComp.get() << std::endl; // *comp = *newComp.get(); + + // When above TODO is addressed, uncomment AddModifiedComponent below + // unless calling SetChanged (which already calls AddModifiedComponent) + // this->dataPtr->AddModifiedComponent(entity); } } } @@ -1390,6 +1416,7 @@ void EntityComponentManager::SetAllComponentsUnchanged() { this->dataPtr->periodicChangedComponents.clear(); this->dataPtr->oneTimeChangedComponents.clear(); + this->dataPtr->modifiedComponents.clear(); } ///////////////////////////////////////////////// @@ -1406,21 +1433,25 @@ void EntityComponentManager::SetChanged( if (typeIter == ecIter->second.end()) return; + ComponentKey key{_type, typeIter->second}; + if (_c == ComponentState::PeriodicChange) { - this->dataPtr->periodicChangedComponents.insert(typeIter->second); - this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); + this->dataPtr->periodicChangedComponents.insert(key); + this->dataPtr->oneTimeChangedComponents.erase(key); } else if (_c == ComponentState::OneTimeChange) { - this->dataPtr->periodicChangedComponents.erase(typeIter->second); - this->dataPtr->oneTimeChangedComponents.insert(typeIter->second); + this->dataPtr->periodicChangedComponents.erase(key); + this->dataPtr->oneTimeChangedComponents.insert(key); } else { - this->dataPtr->periodicChangedComponents.erase(typeIter->second); - this->dataPtr->oneTimeChangedComponents.erase(typeIter->second); + this->dataPtr->periodicChangedComponents.erase(key); + this->dataPtr->oneTimeChangedComponents.erase(key); } + + this->dataPtr->AddModifiedComponent(_entity); } ///////////////////////////////////////////////// @@ -1453,3 +1484,19 @@ void EntityComponentManager::SetEntityCreateOffset(uint64_t _offset) this->dataPtr->entityCount = _offset; } + +///////////////////////////////////////////////// +void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) +{ + if (this->newlyCreatedEntities.find(_entity) + != this->newlyCreatedEntities.end() || + this->toRemoveEntities.find(_entity) != this->toRemoveEntities.end() || + this->modifiedComponents.find(_entity) != this->modifiedComponents.end()) + { + // modified component is already in newlyCreatedEntities + // or toRemoveEntities list + return; + } + + this->modifiedComponents.insert(_entity); +} diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 829637bdf5..76d61174b4 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -1808,7 +1808,7 @@ TEST_P(EntityComponentManagerFixture, State) { // Check the changed state auto changedStateMsg = manager.ChangedState(); - EXPECT_EQ(2, changedStateMsg.entities_size()); + EXPECT_EQ(4, changedStateMsg.entities_size()); const auto &e4Msg = changedStateMsg.entities(0); EXPECT_EQ(e4, e4Msg.id()); @@ -1890,6 +1890,74 @@ TEST_P(EntityComponentManagerFixture, State) } } +///////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, ChangedStateComponents) +{ + // Entity and component + Entity e1{1}; + int e1c0{123}; + std::string e1c1{"string"}; + + // Fill manager with entity + EXPECT_EQ(e1, manager.CreateEntity()); + EXPECT_EQ(1u, manager.EntityCount()); + + // Component + manager.CreateComponent(e1, IntComponent(e1c0)); + + // Serialize into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + ASSERT_EQ(1, stateMsg.entities_size()); + + // Mark entities/components as not new + manager.RunClearNewlyCreatedEntities(); + auto changedStateMsg = manager.ChangedState(); + EXPECT_EQ(0, changedStateMsg.entities_size()); + + // create component + auto compKey = + manager.CreateComponent(e1, StringComponent(e1c1)); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(1, changedStateMsg.entities_size()); + manager.State(stateMsg); + + // Mark components as not new + manager.RunSetAllComponentsUnchanged(); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(0, changedStateMsg.entities_size()); + + // modify component + auto iter = stateMsg.mutable_entities()->find(e1); + ASSERT_TRUE(iter != stateMsg.mutable_entities()->end()); + + msgs::SerializedEntityMap &e1Msg = iter->second; + + auto compIter = e1Msg.mutable_components()->find(compKey.first); + ASSERT_TRUE(compIter != e1Msg.mutable_components()->end()); + + msgs::SerializedComponent &e1c1Msg = compIter->second; + EXPECT_EQ(e1c1, e1c1Msg.component()); + e1c1Msg.set_component("test"); + EXPECT_EQ("test", e1c1Msg.component()); + (*e1Msg.mutable_components())[e1c1Msg.type()] = e1c1Msg; + + manager.SetState(stateMsg); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(1, changedStateMsg.entities_size()); + + // Mark components as not new + manager.RunSetAllComponentsUnchanged(); + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(0, changedStateMsg.entities_size()); + + // remove component + manager.RemoveComponent(e1, StringComponent::typeId); + + changedStateMsg = manager.ChangedState(); + EXPECT_EQ(1, changedStateMsg.entities_size()); +} + ///////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, Descendants) { @@ -2190,6 +2258,11 @@ TEST_P(EntityComponentManagerFixture, SetEntityCreateOffset) manager.SetEntityCreateOffset(1000); Entity entity2 = manager.CreateEntity(); EXPECT_EQ(1001u, entity2); + + // Apply a lower offset, prints warning but goes through. + manager.SetEntityCreateOffset(500); + Entity entity3 = manager.CreateEntity(); + EXPECT_EQ(501u, entity3); } ////////////////////////////////////////////////// diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 91a8aed645..d7c98189c6 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -57,6 +57,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" @@ -536,6 +537,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) this->SetParent(sensorEntity, linkEntity); } + // Particle emitters + for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); + ++emitterIndex) + { + auto emitter = _link->ParticleEmitterByIndex(emitterIndex); + auto emitterEntity = this->CreateEntities(emitter); + + this->SetParent(emitterEntity, linkEntity); + } + return linkEntity; } @@ -671,6 +682,25 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) return visualEntity; } +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::ParticleEmitter *_emitter) +{ + IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::ParticleEmitter)"); + + // Entity + Entity emitterEntity = this->dataPtr->ecm->CreateEntity(); + + // Components + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::ParticleEmitter(convert(*_emitter))); + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::Pose(ResolveSdfPose(_emitter->SemanticPose()))); + this->dataPtr->ecm->CreateComponent(emitterEntity, + components::Name(_emitter->Name())); + + return emitterEntity; +} + ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Collision *_collision) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 923241007d..4bb9d7a58f 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -355,20 +355,32 @@ void SimulationRunner::UpdatePhysicsParams() if (newStepSize != this->stepSize || std::abs(newRTF - this->desiredRtf) > eps) { - this->SetStepSize( - std::chrono::duration_cast( - newStepSize)); - this->desiredRtf = newRTF; - this->updatePeriod = std::chrono::nanoseconds( - static_cast(this->stepSize.count() / this->desiredRtf)); - - this->simTimes.clear(); - this->realTimes.clear(); - // Update physics components - physicsComp->Data().SetMaxStepSize(physicsParams.max_step_size()); - physicsComp->Data().SetRealTimeFactor(newRTF); - this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId, - ComponentState::OneTimeChange); + bool updated = false; + // Make sure the values are valid before setting them + if (newStepSize.count() > 0.0) + { + this->SetStepSize( + std::chrono::duration_cast( + newStepSize)); + physicsComp->Data().SetMaxStepSize(physicsParams.max_step_size()); + updated = true; + } + if (newRTF > 0.0) + { + this->desiredRtf = newRTF; + this->updatePeriod = std::chrono::nanoseconds( + static_cast(this->stepSize.count() / this->desiredRtf)); + physicsComp->Data().SetRealTimeFactor(newRTF); + updated = true; + } + if (updated) + { + this->simTimes.clear(); + this->realTimes.clear(); + // Set as OneTimeChange to make sure the update is not missed + this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId, + ComponentState::OneTimeChange); + } } this->entityCompMgr.RemoveComponent(worldEntity); } diff --git a/src/Util.cc b/src/Util.cc index 5f2e4740f9..d6af1281ea 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -43,6 +43,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" @@ -168,6 +169,10 @@ ComponentTypeId entityTypeId(const Entity &_entity, { type = components::Actor::typeId; } + else if (_ecm.Component(_entity)) + { + type = components::ParticleEmitter::typeId; + } return type; } @@ -214,6 +219,10 @@ std::string entityTypeStr(const Entity &_entity, { type = "actor"; } + else if (_ecm.Component(_entity)) + { + type = "particle_emitter"; + } return type; } @@ -435,6 +444,23 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity, return modelEntity; } +////////////////////////////////////////////////// +std::string topicFromScopedName(const Entity &_entity, + const EntityComponentManager &_ecm, bool _excludeWorld) +{ + std::string topic = scopedName(_entity, _ecm, "/", true); + + if (_excludeWorld) + { + // Exclude the world name. If the entity is a world, then return an + // empty string. + topic = _ecm.Component(_entity) ? "" : + removeParentScope(removeParentScope(topic, "/"), "/"); + } + + return transport::TopicUtils::AsValidTopic("/" + topic); +} + ////////////////////////////////////////////////// std::string validTopic(const std::vector &_topics) { diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index f9edf2543b..15e12002c9 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -28,6 +28,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -272,6 +273,10 @@ TEST_F(UtilTest, EntityTypeId) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::Actor()); EXPECT_EQ(components::Actor::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::ParticleEmitter()); + EXPECT_EQ(components::ParticleEmitter::typeId, entityTypeId(entity, ecm)); } ///////////////////////////////////////////////// @@ -317,6 +322,10 @@ TEST_F(UtilTest, EntityTypeStr) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::Actor()); EXPECT_EQ("actor", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::ParticleEmitter()); + EXPECT_EQ("particle_emitter", entityTypeStr(entity, ecm)); } ///////////////////////////////////////////////// @@ -510,3 +519,93 @@ TEST_F(UtilTest, ValidTopic) EXPECT_EQ("not_bad", validTopic({fixable, invalid, good})); EXPECT_EQ("good", validTopic({invalid, good, fixable})); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, TopicFromScopedName) +{ + EntityComponentManager ecm; + + // world + // - modelA + // - linkA + // - modelB + // - linkB + // - emitterB + // - modelC + + // World + auto worldEntity = ecm.CreateEntity(); + ecm.CreateComponent(worldEntity, components::World()); + ecm.CreateComponent(worldEntity, components::Name("world_name")); + + // Model A + auto modelAEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelAEntity, components::Model()); + ecm.CreateComponent(modelAEntity, components::Name("modelA_name")); + ecm.CreateComponent(modelAEntity, components::ParentEntity(worldEntity)); + + // Link A - Child of Model A + auto linkAEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkAEntity, components::Link()); + ecm.CreateComponent(linkAEntity, components::Name("linkA_name")); + ecm.CreateComponent(linkAEntity, components::ParentEntity(modelAEntity)); + + // Model B - nested inside Model A + auto modelBEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelBEntity, components::Model()); + ecm.CreateComponent(modelBEntity, components::Name("modelB_name")); + ecm.CreateComponent(modelBEntity, components::ParentEntity(modelAEntity)); + + // Link B - child of Model B + auto linkBEntity = ecm.CreateEntity(); + ecm.CreateComponent(linkBEntity, components::Link()); + ecm.CreateComponent(linkBEntity, components::Name("linkB_name")); + ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity)); + + // Emitter B - child of Link B + auto emitterBEntity = ecm.CreateEntity(); + ecm.CreateComponent(emitterBEntity, components::ParticleEmitter()); + ecm.CreateComponent(emitterBEntity, components::Name("emitterB_name")); + ecm.CreateComponent(emitterBEntity, components::ParentEntity(linkBEntity)); + + // Model C + auto modelCEntity = ecm.CreateEntity(); + ecm.CreateComponent(modelCEntity, components::Model()); + ecm.CreateComponent(modelCEntity, components::Name("modelC_name")); + ecm.CreateComponent(modelCEntity, components::ParentEntity(worldEntity)); + + std::string testName = "/model/modelA_name"; + std::string worldName = "/world/world_name"; + // model A, link A, model B, link B and visual B should have + // model A as the top level model + EXPECT_EQ(testName, topicFromScopedName(modelAEntity, ecm)); + EXPECT_EQ(worldName + testName, + topicFromScopedName(modelAEntity, ecm, false)); + + testName += "/link/linkA_name"; + EXPECT_EQ(testName, topicFromScopedName(linkAEntity, ecm)); + EXPECT_EQ(worldName + testName, topicFromScopedName(linkAEntity, ecm, false)); + + testName = "/model/modelA_name/model/modelB_name"; + EXPECT_EQ(testName, topicFromScopedName(modelBEntity, ecm)); + EXPECT_EQ(worldName + testName, + topicFromScopedName(modelBEntity, ecm, false)); + + testName +="/link/linkB_name"; + EXPECT_EQ(testName, topicFromScopedName(linkBEntity, ecm)); + EXPECT_EQ(worldName + testName, topicFromScopedName(linkBEntity, ecm, false)); + + testName += "/particle_emitter/emitterB_name"; + EXPECT_EQ(testName, + topicFromScopedName(emitterBEntity, ecm)); + EXPECT_EQ(worldName + testName, + topicFromScopedName(emitterBEntity, ecm, false)); + + testName = "/model/modelC_name"; + EXPECT_EQ(testName, topicFromScopedName(modelCEntity, ecm)); + EXPECT_EQ(worldName + testName, + topicFromScopedName(modelCEntity, ecm, false)); + + EXPECT_TRUE(topicFromScopedName(worldEntity, ecm).empty()); + EXPECT_EQ(worldName, topicFromScopedName(worldEntity, ecm, false)); +} diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 276b07898a..e802951738 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -1298,6 +1298,21 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id, if (_emitter.has_pose()) emitter->SetLocalPose(msgs::Convert(_emitter.pose())); + // particle scatter ratio + if (_emitter.has_header()) + { + for (int i = 0; i < _emitter.header().data_size(); ++i) + { + const auto &data = _emitter.header().data(i); + const std::string key = "particle_scatter_ratio"; + if (data.key() == "particle_scatter_ratio" && data.value_size() > 0) + { + emitter->SetParticleScatterRatio(math::parseFloat(data.value(0))); + break; + } + } + } + return emitter; } diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index f8b1d3d031..2e2f830236 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -114,8 +114,10 @@ add_subdirectory(magnetometer) add_subdirectory(mecanum_drive) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) +add_subdirectory(odometry_publisher) add_subdirectory(optical_tactile_plugin) add_subdirectory(particle_emitter) +add_subdirectory(particle_emitter2) add_subdirectory(performer_detector) add_subdirectory(physics) add_subdirectory(pose_publisher) diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 656095649d..9830edfbd7 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -169,6 +169,8 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, if (pose) msgs::Set(msg.mutable_pose(), pose->Data()); + static bool hasWarned {false}; + // Process each joint for (const Entity &joint : this->joints) { @@ -190,11 +192,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, for (size_t i = 0; i < jointPositions->Data().size(); ++i) { if (i == 0) + { jointMsg->mutable_axis1()->set_position(jointPositions->Data()[i]); + } else if (i == 1) + { jointMsg->mutable_axis2()->set_position(jointPositions->Data()[i]); - else + } + else if (!hasWarned) + { ignwarn << "Joint state publisher only supports two joint axis\n"; + hasWarned = true; + } } } @@ -206,11 +215,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, for (size_t i = 0; i < jointVelocity->Data().size(); ++i) { if (i == 0) + { jointMsg->mutable_axis1()->set_velocity(jointVelocity->Data()[i]); + } else if (i == 1) + { jointMsg->mutable_axis2()->set_velocity(jointVelocity->Data()[i]); - else + } + else if (!hasWarned) + { ignwarn << "Joint state publisher only supports two joint axis\n"; + hasWarned = true; + } } } @@ -222,11 +238,18 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, for (size_t i = 0; i < jointForce->Data().size(); ++i) { if (i == 0) + { jointMsg->mutable_axis1()->set_force(jointForce->Data()[i]); + } else if (i == 1) + { jointMsg->mutable_axis2()->set_force(jointForce->Data()[i]); - else + } + else if (!hasWarned) + { ignwarn << "Joint state publisher only supports two joint axis\n"; + hasWarned = true; + } } } } diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index e7b557e44d..60769353fe 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -44,6 +44,7 @@ #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/LogPlaybackStatistics.hh" #include "ignition/gazebo/components/Material.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/World.hh" @@ -124,6 +125,9 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \brief Saves which entity poses have changed according to the latest /// LogPlaybackPrivate::Parse call. public: std::unordered_map recentEntityPoseUpdates; + + // \brief Saves which particle emitter emitting components have changed + public: std::unordered_map prevParticleEmitterCmds; }; bool LogPlaybackPrivate::started{false}; @@ -636,6 +640,31 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) return true; }); + // particle emitters + _ecm.Each( + [&](const Entity &_entity, + const components::ParticleEmitterCmd *_emitter) -> bool + { + if (this->dataPtr->prevParticleEmitterCmds.find(_entity) == + this->dataPtr->prevParticleEmitterCmds.end()) + { + this->dataPtr->prevParticleEmitterCmds[_entity] + = _emitter->Data().emitting().data(); + return true; + } + + if (this->dataPtr->prevParticleEmitterCmds[_entity] != + _emitter->Data().emitting().data()) + { + this->dataPtr->prevParticleEmitterCmds[_entity] + = _emitter->Data().emitting().data(); + _ecm.SetChanged(_entity, components::ParticleEmitterCmd::typeId, + ComponentState::OneTimeChange); + } + + return true; + }); + // for seek back in time only // remove entities that should not be present in the current time step for (auto entity : entitiesToRemove) diff --git a/src/systems/odometry_publisher/CMakeLists.txt b/src/systems/odometry_publisher/CMakeLists.txt new file mode 100644 index 0000000000..1b12fd3fde --- /dev/null +++ b/src/systems/odometry_publisher/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_system(odometry-publisher + SOURCES + OdometryPublisher.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc new file mode 100644 index 0000000000..61a6312c0d --- /dev/null +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -0,0 +1,286 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "OdometryPublisher.hh" + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::OdometryPublisherPrivate +{ + /// \brief Calculates odometry and publishes an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief Name of the world-fixed coordinate frame for the odometry message. + public: std::string odomFrame; + + /// \brief Name of the coordinate frame rigidly attached to the mobile + /// robot base. + public: std::string robotBaseFrame; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Diff drive odometry message publisher. + public: transport::Node::Publisher odomPub; + + /// \brief Rolling mean accumulators for the linear velocity + public: std::pair linearMean; + + /// \brief Rolling mean accumulators for the angular velocity + public: math::RollingMean angularMean; + + /// \brief Initialized flag. + public: bool initialized{false}; + + /// \brief Current pose of the model in the odom frame. + public: math::Pose3d lastUpdatePose{0, 0, 0, 0, 0, 0}; + + /// \brief Current timestamp. + public: math::clock::time_point lastUpdateTime; +}; + +////////////////////////////////////////////////// +OdometryPublisher::OdometryPublisher() + : dataPtr(std::make_unique()) +{ + this->dataPtr->linearMean.first.SetWindowSize(10); + this->dataPtr->linearMean.second.SetWindowSize(10); + this->dataPtr->angularMean.SetWindowSize(10); + this->dataPtr->linearMean.first.Clear(); + this->dataPtr->linearMean.second.Clear(); + this->dataPtr->angularMean.Clear(); +} + +////////////////////////////////////////////////// +void OdometryPublisher::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "DiffDrive plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->odomFrame = this->dataPtr->model.Name(_ecm) + "/" + "odom"; + if (!_sdf->HasElement("odom_frame")) + { + ignwarn << "OdometryPublisher system plugin missing , " + << "defaults to \"" << this->dataPtr->odomFrame << "\"" << std::endl; + } + else + { + this->dataPtr->odomFrame = _sdf->Get("odom_frame"); + } + + this->dataPtr->robotBaseFrame = this->dataPtr->model.Name(_ecm) + + "/" + "base_footprint"; + if (!_sdf->HasElement("robot_base_frame")) + { + ignwarn << "OdometryPublisher system plugin missing , " + << "defaults to \"" << this->dataPtr->robotBaseFrame << "\"" << std::endl; + } + else + { + this->dataPtr->robotBaseFrame = _sdf->Get("robot_base_frame"); + } + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration period{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(period); + } + + // Setup odometry + std::string odomTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"}; + if (_sdf->HasElement("odom_topic")) + odomTopic = _sdf->Get("odom_topic"); + std::string odomTopicValid {transport::TopicUtils::AsValidTopic(odomTopic)}; + if (odomTopicValid.empty()) + { + ignerr << "Failed to generate odom topic [" + << odomTopic << "]" << std::endl; + return; + } + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopicValid); +} + +////////////////////////////////////////////////// +void OdometryPublisher::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("OdometryPublisher::PreUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Create the pose component if it does not exist. + auto pos = _ecm.Component( + this->dataPtr->model.Entity()); + if (!pos) + { + _ecm.CreateComponent(this->dataPtr->model.Entity(), + components::Pose()); + } +} + +////////////////////////////////////////////////// +void OdometryPublisher::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("OdometryPublisher::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void OdometryPublisherPrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("DiffDrive::UpdateOdometry"); + // Record start time. + if (!this->initialized) + { + this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); + this->initialized = true; + return; + } + + // Construct the odometry message and publish it. + msgs::Odometry msg; + + const std::chrono::duration dt = + std::chrono::steady_clock::time_point(_info.simTime) - lastUpdateTime; + // We cannot estimate the speed if the time interval is zero (or near + // zero). + if (math::equal(0.0, dt.count())) + return; + + // Get and set robotBaseFrame to odom transformation. + const math::Pose3d pose = worldPose(this->model.Entity(), _ecm); + msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); + msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); + msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); + + // Get linear and angular displacements from last updated pose. + double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); + double linearDisplacementY = pose.Pos().Y() - this->lastUpdatePose.Pos().Y(); + + double currentYaw = pose.Rot().Yaw(); + const double lastYaw = this->lastUpdatePose.Rot().Yaw(); + while (currentYaw < lastYaw - IGN_PI) currentYaw += 2 * IGN_PI; + while (currentYaw > lastYaw + IGN_PI) currentYaw -= 2 * IGN_PI; + const float angularDiff = currentYaw - lastYaw; + + // Get velocities in robotBaseFrame and add to message. + double linearVelocityX = (cosf(currentYaw) * linearDisplacementX + + sinf(currentYaw) * linearDisplacementY) / dt.count(); + double linearVelocityY = (cosf(currentYaw) * linearDisplacementY + - sinf(currentYaw) * linearDisplacementX) / dt.count(); + this->linearMean.first.Push(linearVelocityX); + this->linearMean.second.Push(linearVelocityY); + this->angularMean.Push(angularDiff / dt.count()); + msg.mutable_twist()->mutable_linear()->set_x(this->linearMean.first.Mean()); + msg.mutable_twist()->mutable_linear()->set_y(this->linearMean.second.Mean()); + msg.mutable_twist()->mutable_angular()->set_z(this->angularMean.Mean()); + + // Set the time stamp in the header. + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame ids. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(odomFrame); + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(robotBaseFrame); + + this->lastUpdatePose = pose; + this->lastUpdateTime = std::chrono::steady_clock::time_point(_info.simTime); + + // Throttle publishing. + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + this->odomPub.Publish(msg); +} + +IGNITION_ADD_PLUGIN(OdometryPublisher, + ignition::gazebo::System, + OdometryPublisher::ISystemConfigure, + OdometryPublisher::ISystemPreUpdate, + OdometryPublisher::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(OdometryPublisher, + "ignition::gazebo::systems::OdometryPublisher") diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh new file mode 100644 index 0000000000..f60e309d06 --- /dev/null +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -0,0 +1,91 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_ODOMETRYPUBLISHER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class OdometryPublisherPrivate; + + /// \brief Odometry Publisher which can be attached to any entity in + /// order to periodically publish 2D odometry data in the form of + /// ignition::msgs::Odometry messages. + /// + /// # System Parameters + /// + /// ``: 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 + /// to the mobile robot base. This element is optional, and the default + /// value is `{name_of_model}/base_footprint`. + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element is optional, and the default value is + /// `/model/{name_of_model}/odometry`. + class OdometryPublisher + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: OdometryPublisher(); + + /// \brief Destructor + public: ~OdometryPublisher() 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 ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/particle_emitter/ParticleEmitter.cc b/src/systems/particle_emitter/ParticleEmitter.cc index 6c2de2a435..db1aa6b924 100644 --- a/src/systems/particle_emitter/ParticleEmitter.cc +++ b/src/systems/particle_emitter/ParticleEmitter.cc @@ -258,6 +258,17 @@ void ParticleEmitter::Configure(const Entity &_entity, absolutePath); } + // particle scatter ratio + const std::string scatterRatioKey = "particle_scatter_ratio"; + if (_sdf->HasElement(scatterRatioKey)) + { + // todo(anyone) add particle_scatter_ratio field in next release of ign-msgs + auto data = this->dataPtr->emitter.mutable_header()->add_data(); + data->set_key(scatterRatioKey); + std::string *value = data->add_value(); + *value = _sdf->Get(scatterRatioKey); + } + igndbg << "Loading particle emitter:" << std::endl << this->dataPtr->emitter.DebugString() << std::endl; diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh index 7a9ab1e278..935fbd0eb2 100644 --- a/src/systems/particle_emitter/ParticleEmitter.hh +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -33,6 +33,9 @@ namespace systems /// \brief A system for creating a particle emitter. /// + /// This system will be deprecated in Igition Fortress. Please consider + /// using the ParticleEmitter2 system. + /// /// System parameters /// /// ``: Unique name for the particle emitter. The name will be diff --git a/src/systems/particle_emitter2/CMakeLists.txt b/src/systems/particle_emitter2/CMakeLists.txt new file mode 100644 index 0000000000..3abcb6619d --- /dev/null +++ b/src/systems/particle_emitter2/CMakeLists.txt @@ -0,0 +1,9 @@ +gz_add_system(particle-emitter2 + SOURCES + ParticleEmitter2.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/particle_emitter2/ParticleEmitter2.cc b/src/systems/particle_emitter2/ParticleEmitter2.cc new file mode 100644 index 0000000000..0298e71530 --- /dev/null +++ b/src/systems/particle_emitter2/ParticleEmitter2.cc @@ -0,0 +1,256 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include "ParticleEmitter2.hh" + +using namespace std::chrono_literals; + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +// Private data class. +class ignition::gazebo::systems::ParticleEmitter2Private +{ + /// \brief Callback for receiving particle emitter commands. + /// \param[in] _msg Particle emitter message. + public: void OnCmd(const ignition::msgs::ParticleEmitter &_msg, + const transport::MessageInfo &_info); + + public: bool EmittersService(ignition::msgs::ParticleEmitter_V &_res); + + /// \brief The transport node. + public: ignition::transport::Node node; + + /// \brief Map of topic name to particle emitter entity. + public: std::map emitterTopicMap; + + /// \brief Map of Entity to particle emitter command requested externally. + public: std::map userCmd; + + /// \brief A mutex to protect the user command. + public: std::mutex mutex; + + /// \brief Protects serviceMsg. + public: std::mutex serviceMutex; + + /// \brief Filled on demand for the emitter service. + public: msgs::ParticleEmitter_V serviceMsg; +}; + +////////////////////////////////////////////////// +void ParticleEmitter2Private::OnCmd(const msgs::ParticleEmitter &_msg, + const transport::MessageInfo &_info) +{ + std::lock_guard lock(this->mutex); + std::map::const_iterator iter = + this->emitterTopicMap.find(_info.Topic()); + if (iter != this->emitterTopicMap.end()) + { + this->userCmd[iter->second].CopyFrom(_msg); + } + else + { + ignwarn << "Topic[" << _info.Topic() << "] is not known to the particle " + "emitter system. The requested command will be ignored.\n"; + } +} + +////////////////////////////////////////////////// +bool ParticleEmitter2Private::EmittersService( + ignition::msgs::ParticleEmitter_V &_res) +{ + _res.Clear(); + + std::scoped_lock lock(this->serviceMutex); + _res.CopyFrom(this->serviceMsg); + return true; +} + +////////////////////////////////////////////////// +ParticleEmitter2::ParticleEmitter2() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ParticleEmitter2::Configure(const Entity &_entity, + const std::shared_ptr & /*_sdf*/, + EntityComponentManager &_ecm, + EventManager & /*_eventMgr*/) +{ + // World + const components::Name *name = _ecm.Component(_entity); + if (name == nullptr) + { + ignerr << "World with id: " << _entity + << " has no name. ParticleEmitter2 cannot create transport topics\n"; + return; + } + + std::string emittersService = "/world/" + name->Data() + "/particle_emitters"; + if (this->dataPtr->node.Advertise(emittersService, + &ParticleEmitter2Private::EmittersService, this->dataPtr.get())) + { + ignmsg << "Serving particle emitter information on [" + << emittersService << "]" << std::endl; + } + else + { + ignerr << "Something went wrong, failed to advertise [" << emittersService + << "]" << std::endl; + } +} + +////////////////////////////////////////////////// +void ParticleEmitter2::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("ParticleEmitter2::PreUpdate"); + + std::lock_guard lock(this->dataPtr->mutex); + + // Create particle emitters + { + std::lock_guard serviceLock(this->dataPtr->serviceMutex); + _ecm.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent, + const components::Pose *_pose)->bool + { + std::string topic; + + // Get the topic information from the header, which is currently a + // hack to avoid breaking the particle_emitter.proto message. + if (_emitter->Data().has_header()) + { + for (const auto data : _emitter->Data().header().data()) + { + if (data.key() == "topic" && !data.value().empty()) + { + topic = data.value(0); + } + } + } + + // If a topic has not been specified, then generate topic based + // on the scoped name. + topic = !topic.empty() ? topic : + topicFromScopedName(_entity, _ecm) + "/cmd"; + + // Subscribe to the topic that receives particle emitter commands. + if (!this->dataPtr->node.Subscribe( + topic, &ParticleEmitter2Private::OnCmd, this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << topic << "]. " + << "Particle emitter will not receive updates." << std::endl; + return false; + } + ignmsg << "Particle emitter[" + << scopedName(_entity, _ecm, "::", false) << "] subscribed " + << "to command messages on topic[" << topic << "]\n"; + + // Store the topic name so that we can apply user commands + // correctly. + this->dataPtr->emitterTopicMap[topic] = _entity; + + // Store the emitter information in the service message, which + // can then be used in the particle_emitters service. + msgs::ParticleEmitter *emitterMsg = + this->dataPtr->serviceMsg.add_particle_emitter(); + emitterMsg->CopyFrom(_emitter->Data()); + msgs::Set(emitterMsg->mutable_pose(), _pose->Data()); + + // Set the topic information if it was not set via SDF. + if (!emitterMsg->has_header()) + { + auto headerData = emitterMsg->mutable_header()->add_data(); + headerData->set_key("topic"); + headerData->add_value(topic); + } + + // Set the particle emitter frame + auto frameData = emitterMsg->mutable_header()->add_data(); + frameData->set_key("frame"); + frameData->add_value( + removeParentScope( + scopedName(_parent->Data(), _ecm, "::", false), "::")); + + return true; + }); + } + + if (this->dataPtr->userCmd.empty() || _info.paused) + return; + + // Process each command + for (const auto cmd : this->dataPtr->userCmd) + { + // Create component. + auto emitterComp = _ecm.Component( + cmd.first); + if (!emitterComp) + { + _ecm.CreateComponent(cmd.first, + components::ParticleEmitterCmd(cmd.second)); + } + else + { + emitterComp->Data() = cmd.second; + + // Note: we process the cmd component in RenderUtil but if there is only + // rendering on the gui side, it will not be able to remove the cmd + // component from the ECM. It seems like adding OneTimeChange here will + // make sure the cmd component is found again in Each call on GUI side. + // todo(anyone) find a better way to process this cmd component in + // RenderUtil.cc + _ecm.SetChanged(cmd.first, + components::ParticleEmitterCmd::typeId, + ComponentState::OneTimeChange); + } + + } + this->dataPtr->userCmd.clear(); +} + +IGNITION_ADD_PLUGIN(ParticleEmitter2, + ignition::gazebo::System, + ParticleEmitter2::ISystemConfigure, + ParticleEmitter2::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ParticleEmitter2, + "ignition::gazebo::systems::ParticleEmitter2") diff --git a/src/systems/particle_emitter2/ParticleEmitter2.hh b/src/systems/particle_emitter2/ParticleEmitter2.hh new file mode 100644 index 0000000000..a52d39f65f --- /dev/null +++ b/src/systems/particle_emitter2/ParticleEmitter2.hh @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER2_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER2_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class ParticleEmitter2Private; + + /// \brief A system for running and managing particle emitters. A particle + /// emitter is defined using the SDF element. + /// + /// This system will create a transport subscriber for each + /// 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` + /// + /// \todo(nkoenig) Plan for ParticleEmitter and ParticleEmitter2: + /// 1. Deprecate ParticleEmitter in Ignition Fortress. + /// 2. Remove ParticleEmitter in Ignition G. + /// 3. Copy ParticleEmitter2 to ParticleEmitter in Ignition G. + /// 4. Deprecrate ParticleEmitter2 in Ignition G. + /// 5. Remove ParticleEmitter2 in Ignition H. + class ParticleEmitter2 + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ParticleEmitter2(); + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif + diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 5aeac72fae..2aaea50da8 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -324,16 +324,20 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief Feature list to handle collisions. public: struct CollisionFeatureList : ignition::physics::FeatureList< MinimumFeatureList, - ignition::physics::GetContactsFromLastStepFeature, ignition::physics::sdf::ConstructSdfCollision>{}; + /// \brief Feature list to handle contacts information. + public: struct ContactFeatureList : ignition::physics::FeatureList< + CollisionFeatureList, + ignition::physics::GetContactsFromLastStepFeature>{}; + /// \brief Collision type with collision features. public: using ShapePtrType = ignition::physics::ShapePtr< ignition::physics::FeaturePolicy3d, CollisionFeatureList>; /// \brief World type with just the minimum features. Non-pointer. public: using WorldShapeType = ignition::physics::World< - ignition::physics::FeaturePolicy3d, CollisionFeatureList>; + ignition::physics::FeaturePolicy3d, ContactFeatureList>; ////////////////////////////////////////////////// // Collision filtering with bitmasks @@ -395,6 +399,7 @@ class ignition::gazebo::systems::PhysicsPrivate physics::World, MinimumFeatureList, CollisionFeatureList, + ContactFeatureList, NestedModelFeatureList>; /// \brief A map between world entity ids in the ECM to World Entities in @@ -442,6 +447,7 @@ class ignition::gazebo::systems::PhysicsPrivate public: using EntityCollisionMap = EntityFeatureMap3d< physics::Shape, CollisionFeatureList, + ContactFeatureList, CollisionMaskFeatureList, FrictionPyramidSlipComplianceFeatureList >; @@ -1625,6 +1631,111 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); + // Update link angular velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::AngularVelocityCmd *_angularVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldAngularVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + + if (!worldAngularVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link angular velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldAngularVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _angularVelocityCmd->Data(); + worldAngularVelFeature->SetWorldAngularVelocity( + math::eigen3::convert(worldAngularVel)); + + return true; + }); + + // Update link linear velocity + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::LinearVelocityCmd *_linearVelocityCmd) + { + if (!this->entityLinkMap.HasEntity(_entity)) + { + ignwarn << "Failed to find link [" << _entity + << "]." << std::endl; + return true; + } + + auto linkPtrPhys = this->entityLinkMap.Get(_entity); + if (nullptr == linkPtrPhys) + return true; + + auto freeGroup = linkPtrPhys->FindFreeGroup(); + if (!freeGroup) + return true; + this->entityFreeGroupMap.AddEntity(_entity, freeGroup); + + auto worldLinearVelFeature = + this->entityFreeGroupMap + .EntityCast(_entity); + if (!worldLinearVelFeature) + { + static bool informed{false}; + if (!informed) + { + igndbg << "Attempting to set link linear velocity, but the " + << "physics engine doesn't support velocity commands. " + << "Velocity won't be set." + << std::endl; + informed = true; + } + return true; + } + + // velocity in world frame = world_to_model_tf * model_to_link_tf * vel + Entity modelEntity = topLevelModel(_entity, _ecm); + const components::Pose *modelEntityPoseComp = + _ecm.Component(modelEntity); + math::Pose3d modelToLinkTransform = this->RelativePose( + modelEntity, _entity, _ecm); + math::Vector3d worldLinearVel = modelEntityPoseComp->Data().Rot() + * modelToLinkTransform.Rot() * _linearVelocityCmd->Data(); + worldLinearVelFeature->SetWorldLinearVelocity( + math::eigen3::convert(worldLinearVel)); + + return true; + }); + + // Populate bounding box info // Only compute bounding box if component exists to avoid unnecessary // computations @@ -2209,6 +2320,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, }); IGN_PROFILE_END(); + _ecm.Each( + [&](const Entity &, components::AngularVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + + _ecm.Each( + [&](const Entity &, components::LinearVelocityCmd *_vel) -> bool + { + _vel->Data() = math::Vector3d::Zero; + return true; + }); + // Update joint positions IGN_PROFILE_BEGIN("Joints"); _ecm.Each( @@ -2277,14 +2402,14 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) } auto worldCollisionFeature = - this->entityWorldMap.EntityCast(worldEntity); + this->entityWorldMap.EntityCast(worldEntity); if (!worldCollisionFeature) { static bool informed{false}; if (!informed) { igndbg << "Attempting process contacts, but the physics " - << "engine doesn't support collision features. " + << "engine doesn't support contact features. " << "Contacts won't be computed." << std::endl; informed = true; @@ -2311,8 +2436,10 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { const auto &contact = contactComposite.Get(); - auto coll1Entity = this->entityCollisionMap.Get(contact.collision1); - auto coll2Entity = this->entityCollisionMap.Get(contact.collision2); + auto coll1Entity = + this->entityCollisionMap.Get(ShapePtrType(contact.collision1)); + auto coll2Entity = + this->entityCollisionMap.Get(ShapePtrType(contact.collision2)); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 2dbaaf61be..c299ca50f0 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -38,6 +38,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -124,6 +125,15 @@ class ignition::gazebo::systems::SceneBroadcasterPrivate public: static void AddVisuals(msgs::Link *_msg, const Entity _entity, const SceneGraphType &_graph); + /// \brief Adds sensors to a msgs::Link object based on the contents of + /// the scene graph + /// \param[inout] _msg Pointer to msg object to which the sensors will be + /// added. + /// \param[in] _entity Parent entity in the graph + /// \param[in] _graph Scene graph + public: static void AddSensors(msgs::Link *_msg, const Entity _entity, + const SceneGraphType &_graph); + /// \brief Recursively remove entities from the graph /// \param[in] _entity Entity /// \param[in/out] _graph Scene graph @@ -742,6 +752,26 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( return true; }); + // Sensors + _manager.EachNew( + [&](const Entity &_entity, const components::Sensor *, + const components::Name *_nameComp, + const components::ParentEntity *_parentComp, + const components::Pose *_poseComp) -> bool + { + auto sensorMsg = std::make_shared(); + sensorMsg->set_id(_entity); + sensorMsg->set_parent_id(_parentComp->Data()); + sensorMsg->set_name(_nameComp->Data()); + sensorMsg->mutable_pose()->CopyFrom(msgs::Convert(_poseComp->Data())); + + // Add to graph + newGraph.AddVertex(_nameComp->Data(), sensorMsg, _entity); + newGraph.AddEdge({_parentComp->Data(), _entity}, true); + newEntity = true; + return true; + }); // Update the whole scene graph from the new graph { @@ -886,6 +916,24 @@ void SceneBroadcasterPrivate::AddVisuals(msgs::Link *_msg, const Entity _entity, } } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::AddSensors(msgs::Link *_msg, const Entity _entity, + const SceneGraphType &_graph) +{ + if (!_msg) + return; + + for (const auto &vertex : _graph.AdjacentsFrom(_entity)) + { + auto sensorMsg = std::dynamic_pointer_cast( + vertex.second.get().Data()); + if (!sensorMsg) + continue; + + _msg->add_sensor()->CopyFrom(*sensorMsg); + } +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, const SceneGraphType &_graph) @@ -908,6 +956,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, // Lights AddLights(msgOut, vertex.second.get().Id(), _graph); + + // Sensors + AddSensors(msgOut, vertex.second.get().Id(), _graph); } } diff --git a/src/systems/velocity_control/VelocityControl.cc b/src/systems/velocity_control/VelocityControl.cc index 4ba40560ec..8082c39716 100644 --- a/src/systems/velocity_control/VelocityControl.cc +++ b/src/systems/velocity_control/VelocityControl.cc @@ -15,9 +15,11 @@ * */ +#include #include #include #include +#include #include #include @@ -37,10 +39,15 @@ using namespace systems; class ignition::gazebo::systems::VelocityControlPrivate { - /// \brief Callback for velocity subscription + /// \brief Callback for model velocity subscription /// \param[in] _msg Velocity message public: void OnCmdVel(const ignition::msgs::Twist &_msg); + /// \brief Callback for link velocity subscription + /// \param[in] _msg Velocity message + public: void OnLinkCmdVel(const ignition::msgs::Twist &_msg, + const ignition::transport::MessageInfo &_info); + /// \brief Update the linear and angular velocities. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -48,23 +55,45 @@ class ignition::gazebo::systems::VelocityControlPrivate public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Update link velocity. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateLinkVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + /// \brief Ignition communication node. public: transport::Node node; + /// \brief Model interface + public: Model model{kNullEntity}; + /// \brief Angular velocity of a model public: math::Vector3d angularVelocity{0, 0, 0}; /// \brief Linear velocity of a model public: math::Vector3d linearVelocity{0, 0, 0}; - /// \brief Model interface - public: Model model{kNullEntity}; - /// \brief Last target velocity requested. public: msgs::Twist targetVel; - /// \brief A mutex to protect the target velocity command. + /// \brief A mutex to protect the model velocity command. public: std::mutex mutex; + + /// \brief Link names + public: std::vector linkNames; + + /// \brief Link entities in a model + public: std::unordered_map links; + + /// \brief Angular velocities of links + public: std::unordered_map angularVelocities; + + /// \brief Linear velocities of links + public: std::unordered_map linearVelocities; + + /// \brief All link velocites + public: std::unordered_map linkVels; }; ////////////////////////////////////////////////// @@ -107,19 +136,47 @@ void VelocityControl::Configure(const Entity &_entity, << this->dataPtr->angularVelocity << "]" << std::endl; } - // Subscribe to commands - std::vector topics; + // Subscribe to model commands + std::vector modelTopics; if (_sdf->HasElement("topic")) { - topics.push_back(_sdf->Get("topic")); + modelTopics.push_back(_sdf->Get("topic")); } - topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); - auto topic = validTopic(topics); + modelTopics.push_back( + "/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + auto modelTopic = validTopic(modelTopics); this->dataPtr->node.Subscribe( - topic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); - - ignmsg << "VelocityControl subscribing to twist messages on [" << topic << "]" + modelTopic, &VelocityControlPrivate::OnCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << modelTopic << "]" << std::endl; + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + if (!ptr->HasElement("link_name")) + return; + + sdf::ElementPtr sdfElem = ptr->GetElement("link_name"); + while (sdfElem) + { + this->dataPtr->linkNames.push_back(sdfElem->Get()); + sdfElem = sdfElem->GetNextElement("link_name"); + } + + // Subscribe to link commands + for (const auto &linkName : this->dataPtr->linkNames) + { + std::string linkTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + linkName + "/cmd_vel"}; + linkTopic = transport::TopicUtils::AsValidTopic(linkTopic); + this->dataPtr->node.Subscribe( + linkTopic, &VelocityControlPrivate::OnLinkCmdVel, this->dataPtr.get()); + ignmsg << "VelocityControl subscribing to twist messages on [" + << linkTopic << "]" + << std::endl; + } } ////////////////////////////////////////////////// @@ -141,11 +198,11 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return; // update angular velocity of model - auto angularVel = + auto modelAngularVel = _ecm.Component( this->dataPtr->model.Entity()); - if (angularVel == nullptr) + if (modelAngularVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -153,16 +210,16 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *angularVel = + *modelAngularVel = components::AngularVelocityCmd({this->dataPtr->angularVelocity}); } // update linear velocity of model - auto linearVel = + auto modelLinearVel = _ecm.Component( this->dataPtr->model.Entity()); - if (linearVel == nullptr) + if (modelLinearVel == nullptr) { _ecm.CreateComponent( this->dataPtr->model.Entity(), @@ -170,9 +227,84 @@ void VelocityControl::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } else { - *linearVel = + *modelLinearVel = components::LinearVelocityCmd({this->dataPtr->linearVelocity}); } + + // If there are links, create link components + // If the link hasn't been identified yet, look for it + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->linkNames.empty()) + return; + + // find all the link entity ids + if (this->dataPtr->links.size() != this->dataPtr->linkNames.size()) + { + for (const auto &linkName : this->dataPtr->linkNames) + { + if (this->dataPtr->links.find(linkName) == this->dataPtr->links.end()) + { + Entity link = this->dataPtr->model.LinkByName(_ecm, linkName); + if (link != kNullEntity) + this->dataPtr->links.insert({linkName, link}); + else + { + ignwarn << "Failed to find link [" << linkName + << "] for model [" << modelName << "]" << std::endl; + } + } + } + } + + // update link velocities + for (const auto& [linkName, angularVel] : this->dataPtr->angularVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkAngularVelComp = + _ecm.Component(it->second); + if (!linkAngularVelComp) + { + _ecm.CreateComponent(it->second, + components::AngularVelocityCmd({angularVel})); + } + else + { + *linkAngularVelComp = components::AngularVelocityCmd(angularVel); + } + } + else + { + ignwarn << "No link found for angular velocity cmd [" + << linkName << "]" << std::endl; + } + } + + for (const auto& [linkName, linearVel] : this->dataPtr->linearVelocities) + { + auto it = this->dataPtr->links.find(linkName); + if (it != this->dataPtr->links.end()) + { + auto linkLinearVelComp = + _ecm.Component(it->second); + if (!linkLinearVelComp) + { + _ecm.CreateComponent(it->second, + components::LinearVelocityCmd({linearVel})); + } + else + { + *linkLinearVelComp = components::LinearVelocityCmd(linearVel); + } + } + else + { + ignwarn << "No link found for linear velocity cmd [" + << linkName << "]" << std::endl; + } + } } ////////////////////////////////////////////////// @@ -184,10 +316,12 @@ void VelocityControl::PostUpdate(const UpdateInfo &_info, if (_info.paused) return; + // update model velocities this->dataPtr->UpdateVelocity(_info, _ecm); + // update link velocities + this->dataPtr->UpdateLinkVelocity(_info, _ecm); } - ////////////////////////////////////////////////// void VelocityControlPrivate::UpdateVelocity( const ignition::gazebo::UpdateInfo &/*_info*/, @@ -195,18 +329,27 @@ void VelocityControlPrivate::UpdateVelocity( { IGN_PROFILE("VeocityControl::UpdateVelocity"); - double linVel; - double angVel; + std::lock_guard lock(this->mutex); + this->linearVelocity = msgs::Convert(this->targetVel.linear()); + this->angularVelocity = msgs::Convert(this->targetVel.angular()); +} + +////////////////////////////////////////////////// +void VelocityControlPrivate::UpdateLinkVelocity( + const ignition::gazebo::UpdateInfo &/*_info*/, + const ignition::gazebo::EntityComponentManager &/*_ecm*/) +{ + IGN_PROFILE("VelocityControl::UpdateLinkVelocity"); + + std::lock_guard lock(this->mutex); + for (const auto& [linkName, msg] : this->linkVels) { - std::lock_guard lock(this->mutex); - linVel = this->targetVel.linear().x(); - angVel = this->targetVel.angular().z(); + auto linearVel = msgs::Convert(msg.linear()); + auto angularVel = msgs::Convert(msg.angular()); + this->linearVelocities[linkName] = linearVel; + this->angularVelocities[linkName] = angularVel; } - - this->linearVelocity = math::Vector3d( - linVel, this->targetVel.linear().y(), this->targetVel.linear().z()); - this->angularVelocity = math::Vector3d( - this->targetVel.angular().x(), this->targetVel.angular().y(), angVel); + this->linkVels.clear(); } ////////////////////////////////////////////////// @@ -216,6 +359,21 @@ void VelocityControlPrivate::OnCmdVel(const msgs::Twist &_msg) this->targetVel = _msg; } +////////////////////////////////////////////////// +void VelocityControlPrivate::OnLinkCmdVel(const msgs::Twist &_msg, + const transport::MessageInfo &_info) +{ + std::lock_guard lock(this->mutex); + for (const auto &linkName : this->linkNames) + { + if (_info.Topic().find("/" + linkName + "/cmd_vel") != std::string::npos) + { + this->linkVels.insert({linkName, _msg}); + break; + } + } +} + IGNITION_ADD_PLUGIN(VelocityControl, ignition::gazebo::System, VelocityControl::ISystemConfigure, diff --git a/test/benchmark/README.md b/test/benchmark/README.md index 2fb4934c99..a5f7f79b8a 100644 --- a/test/benchmark/README.md +++ b/test/benchmark/README.md @@ -14,24 +14,24 @@ Given a set of changes to the codebase, it is often useful to see the difference in performance. -Once two (or more) benchmarks have been executed, compare the results with: +Once two (or more) benchmarks have been executed, compare the results by downloading the [benchmark tools](https://github.com/google/benchmark/tree/master/tools), and then run the following command: - ``` - # From the build folder - ../test/gbenchmark/tools/compare.py benchmarks baseline.json contender.json - ``` +``` +# From the downloaded "tools" folder +./compare.py benchmarks baseline.json contender.json +``` ### CPU Scaling Warnings Note: If you receive warnings about CPU scaling, you can change the CPU governor with: - ``` - sudo cpupower frequency-set --governor performance - ``` +``` +sudo cpupower frequency-set --governor performance +``` The previous setting can be restored with: - ``` - sudo cpupower frequency-set --governor powersave - ``` +``` +sudo cpupower frequency-set --governor powersave +``` diff --git a/test/benchmark/each.cc b/test/benchmark/each.cc index e5f5f5336a..a386633c4c 100644 --- a/test/benchmark/each.cc +++ b/test/benchmark/each.cc @@ -54,13 +54,13 @@ class EntityComponentManagerFixture: public benchmark::Fixture { Entity worldEntity = mgr->CreateEntity(); mgr->CreateComponent(worldEntity, World()); - mgr->CreateComponent(worldEntity, Name("world_name")); + mgr->CreateComponent(worldEntity, components::Name("world_name")); } for (int i = 0; i < _nonmatchingEntityCount; ++i) { Entity worldEntity = mgr->CreateEntity(); - mgr->CreateComponent(worldEntity, Name("world_name")); + mgr->CreateComponent(worldEntity, components::Name("world_name")); } } @@ -77,8 +77,8 @@ BENCHMARK_DEFINE_F(EntityComponentManagerFixture, EachNoCache) { int entitiesMatched = 0; - mgr->EachNoCache( - [&](const Entity &, const World *, const Name *)->bool + mgr->EachNoCache( + [&](const Entity &, const World *, const components::Name *)->bool { entitiesMatched++; return true; @@ -102,8 +102,8 @@ BENCHMARK_DEFINE_F(EntityComponentManagerFixture, EachCache) { int entitiesMatched = 0; - mgr->Each( - [&](const Entity &, const World *, const Name *)->bool + mgr->Each( + [&](const Entity &, const World *, const components::Name *)->bool { entitiesMatched++; return true; @@ -131,7 +131,7 @@ class ManyComponentFixture: public benchmark::Fixture for (int i = 0; i < _entityCount; ++i) { Entity entity = mgr->CreateEntity(); - mgr->CreateComponent(entity, Name("world_name")); + mgr->CreateComponent(entity, components::Name("world_name")); mgr->CreateComponent(entity, AngularVelocity()); mgr->CreateComponent(entity, WorldAngularVelocity()); mgr->CreateComponent(entity, Inertial()); @@ -158,8 +158,8 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each1ComponentCache) { int entitiesMatched = 0; - mgr->Each( - [&](const Entity &, const Name *)->bool + mgr->Each( + [&](const Entity &, const components::Name *)->bool { entitiesMatched++; return true; @@ -184,13 +184,13 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each5ComponentCache) { int entitiesMatched = 0; - mgr->EachEach( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const Inertial *, const LinearAcceleration *, @@ -219,7 +219,7 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each10ComponentCache) { int entitiesMatched = 0; - mgr->EachEach( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const WorldAngularVelocity *, const Inertial *, @@ -264,8 +264,8 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each1ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCache( - [&](const Entity &, const Name *)->bool + mgr->EachNoCache( + [&](const Entity &, const components::Name *)->bool { entitiesMatched++; return true; @@ -290,13 +290,13 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each5ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCacheEachNoCache( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const Inertial *, const LinearAcceleration *, @@ -325,7 +325,7 @@ BENCHMARK_DEFINE_F(ManyComponentFixture, Each10ComponentNoCache) { int entitiesMatched = 0; - mgr->EachNoCacheEachNoCache( [&](const Entity &, - const Name *, + const components::Name *, const AngularVelocity *, const WorldAngularVelocity *, const Inertial *, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f81dd21d65..60d0bc4e7c 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -37,7 +37,9 @@ set(tests multiple_servers.cc nested_model_physics.cc network_handshake.cc + odometry_publisher.cc particle_emitter.cc + particle_emitter2.cc performer_detector.cc physics_system.cc play_pause.cc diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 942ce45c8a..30223d0284 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -304,11 +304,15 @@ TEST_P(DiffDriveTest, SkidPublishCmd) EXPECT_EQ(3u, odomPoses.size()); EXPECT_LT(poses[0].Pos().X(), poses[3999].Pos().X()); - EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); EXPECT_NEAR(poses[0].Pos().Z(), poses[3999].Pos().Z(), tol); EXPECT_NEAR(poses[0].Rot().X(), poses[3999].Rot().X(), tol); EXPECT_NEAR(poses[0].Rot().Y(), poses[3999].Rot().Y(), tol); + + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ + EXPECT_LT(poses[0].Pos().Y(), poses[3999].Pos().Y()); EXPECT_LT(poses[0].Rot().Z(), poses[3999].Rot().Z()); +#endif } ///////////////////////////////////////////////// diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index d27122ebfb..5452a923c6 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -780,7 +780,27 @@ TEST_F(LogSystemTest, RecordAndPlayback) stateMsg.ParseFromString(recordedIter->Data()); // entity size = 28 in dbl pendulum + 4 in nested model EXPECT_EQ(32, stateMsg.entities_size()); - EXPECT_EQ(batch.end(), ++recordedIter); + EXPECT_NE(batch.end(), ++recordedIter); + + // check rest of recordIter (state message) for poses + while (batch.end() != recordedIter) + { + EXPECT_EQ("ignition.msgs.SerializedStateMap", recordedIter->Type()); + EXPECT_EQ(recordedIter->Topic(), "/world/log_pendulum/changed_state"); + + stateMsg.ParseFromString(recordedIter->Data()); + + for (const auto &entityIter : stateMsg.entities()) + { + for (const auto &compIter : entityIter.second.components()) + { + EXPECT_EQ(components::Pose::typeId, compIter.second.type()); + } + } + ++recordedIter; + } + + EXPECT_EQ(batch.end(), recordedIter); // Keep track of total number of pose comparisons int nTotal{0}; diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc new file mode 100644 index 0000000000..150f4f3032 --- /dev/null +++ b/test/integration/odometry_publisher.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/AngularVelocityCmd.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" +#include "ignition/gazebo/components/LinearVelocityCmd.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define tol 0.005 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test OdometryPublisher system +class OdometryPublisherTest : public ::testing::TestWithParam +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestMovement(const std::string &_sdfFile, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses and velocities + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server while model is stationary + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // 50 Hz is the default publishing freq + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::vector odomLinVels; + std::vector odomAngVels; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + odomLinVels.push_back(msgs::Convert(_msg.twist().linear())); + odomAngVels.push_back(msgs::Convert(_msg.twist().angular())); + }; + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + test::Relay velocityRamp; + math::Vector3d linVelCmd(1, 0.5, 0.0); + math::Vector3d angVelCmd(0.0, 0.0, 0.2); + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + gazebo::EntityComponentManager &_ecm) + { + auto en = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle")); + EXPECT_NE(kNullEntity, en); + + // Set the linear velocity of the model + auto linVelCmdComp = + _ecm.Component(en); + if (!linVelCmdComp) + { + _ecm.CreateComponent(en, + components::LinearVelocityCmd(linVelCmd)); + } + else + { + linVelCmdComp->Data() = linVelCmd; + } + + // Set the angular velocity of the model + auto angVelCmdComp = + _ecm.Component(en); + if (!angVelCmdComp) + { + _ecm.CreateComponent(en, + components::AngularVelocityCmd(angVelCmd)); + } + else + { + angVelCmdComp->Data() = angVelCmd; + } + }); + + server.AddSystem(velocityRamp.systemPtr); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + EXPECT_EQ(150u, odomLinVels.size()); + EXPECT_EQ(150u, odomAngVels.size()); + + // Check accuracy of poses published in the odometry message + auto finalModelFramePose = odomPoses.back(); + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Z(), odomPoses[0].Pos().Z(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().X(), odomPoses[0].Rot().X(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Y(), odomPoses[0].Rot().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Z(), odomPoses[0].Rot().Z(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Z(), finalModelFramePose.Pos().Z(), 1e-2); + EXPECT_NEAR(poses.back().Rot().X(), finalModelFramePose.Rot().X(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Y(), finalModelFramePose.Rot().Y(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Z(), finalModelFramePose.Rot().Z(), 1e-2); + + // Check accuracy of velocities published in the odometry message + EXPECT_NEAR(odomLinVels[5].X(), linVelCmd[0], 1e-1); + EXPECT_NEAR(odomLinVels[5].Y(), linVelCmd[1], 1e-1); + EXPECT_NEAR(odomLinVels[5].Z(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].X(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].Y(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels[5].Z(), angVelCmd[2], 1e-1); + + EXPECT_NEAR(odomLinVels.back().X(), linVelCmd[0], 1e-1); + EXPECT_NEAR(odomLinVels.back().Y(), linVelCmd[1], 1e-1); + EXPECT_NEAR(odomLinVels.back().Z(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().X(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().Y(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().Z(), angVelCmd[2], 1e-1); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + /// \param[in] _frameId Name of the world-fixed coordinate frame + /// for the odometry message. + /// \param[in] _childFrameId Name of the coordinate frame rigidly + /// attached to the mobile robot base. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_odomTopic, + const std::string &_frameId, + const std::string &_childFrameId) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function odomCb = + [&odomPosesCount, &_frameId, &_childFrameId](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + ASSERT_GT(_msg.header().data_size(), 1); + + EXPECT_STREQ(_msg.header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.header().data(0).value().Get(0).c_str(), + _frameId.c_str()); + + EXPECT_STREQ(_msg.header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.header().data(1).value().Get(0).c_str(), + _childFrameId.c_str()); + + odomPosesCount++; + }; + + transport::Node node; + node.Subscribe(_odomTopic, odomCb); + + server.Run(true, 100, false); + + int sleep = 0; + int maxSleep = 30; + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + EXPECT_EQ(5u, odomPosesCount); + } +}; + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, Movement) +{ + TestMovement( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/odometry_publisher.sdf", + "/model/vehicle/odometry"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, MovementCustomTopic) +{ + TestMovement( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_publisher_custom.sdf", + "/model/bar/odom"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, OdomFrameId) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/odometry_publisher.sdf", + "/model/vehicle/odometry", + "vehicle/odom", + "vehicle/base_footprint"); +} + +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, OdomCustomFrameId) +{ + TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/odometry_publisher_custom.sdf", + "/model/bar/odom", + "odomCustom", + "baseCustom"); +} + +// Run multiple times +INSTANTIATE_TEST_SUITE_P(ServerRepeat, OdometryPublisherTest, + ::testing::Range(1, 2)); diff --git a/test/integration/particle_emitter.cc b/test/integration/particle_emitter.cc index 02981ba444..fba5a782e3 100644 --- a/test/integration/particle_emitter.cc +++ b/test/integration/particle_emitter.cc @@ -122,6 +122,24 @@ TEST_F(ParticleEmitterTest, SDFLoad) // and let rendering do the findFile instead EXPECT_EQ(std::string(), _emitter->Data().color_range_image().data()); + + // particle scatter ratio is temporarily stored in header + bool hasParticleScatterRatio = false; + for (int i = 0; i < _emitter->Data().header().data_size(); ++i) + { + for (int j = 0; + j < _emitter->Data().header().data(i).value_size(); ++j) + { + if (_emitter->Data().header().data(i).key() == + "particle_scatter_ratio") + { + EXPECT_DOUBLE_EQ(0.01, math::parseFloat( + _emitter->Data().header().data(i).value(0))); + hasParticleScatterRatio = true; + } + } + } + EXPECT_TRUE(hasParticleScatterRatio); } else { diff --git a/test/integration/particle_emitter2.cc b/test/integration/particle_emitter2.cc new file mode 100644 index 0000000000..1550c550c6 --- /dev/null +++ b/test/integration/particle_emitter2.cc @@ -0,0 +1,163 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/test_config.hh" + +#include "helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +class ParticleEmitter2Test : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + } + public: void LoadWorld(const std::string &_path, bool _useLevels = false) + { + this->serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, _path)); + this->serverConfig.SetUseLevels(_useLevels); + + this->server = std::make_unique(this->serverConfig); + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + using namespace std::chrono_literals; + this->server->SetUpdatePeriod(1ns); + } + + public: ServerConfig serverConfig; + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// Load an SDF with a particle emitter and verify its properties. +TEST_F(ParticleEmitter2Test, SDFLoad) +{ + bool updateCustomChecked{false}; + bool updateDefaultChecked{false}; + + this->LoadWorld("test/worlds/particle_emitter2.sdf"); + + // Create a system that checks a particle emitter. + test::Relay testSystem; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + + if (_name->Data() == "smoke_emitter") + { + updateCustomChecked = true; + + EXPECT_EQ("smoke_emitter", _name->Data()); + EXPECT_EQ(_name->Data(), _emitter->Data().name()); + EXPECT_EQ(msgs::ParticleEmitter_EmitterType_BOX, + _emitter->Data().type()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), + msgs::Convert(_emitter->Data().pose())); + EXPECT_EQ(math::Vector3d(2, 2, 2), + msgs::Convert(_emitter->Data().size())); + EXPECT_DOUBLE_EQ(5.0, _emitter->Data().rate().data()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration().data()); + EXPECT_TRUE(_emitter->Data().emitting().data()); + EXPECT_EQ(math::Vector3d(3, 3, 3), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime().data()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(10.0, + _emitter->Data().min_velocity().data()); + EXPECT_DOUBLE_EQ(20.0, + _emitter->Data().max_velocity().data()); + EXPECT_EQ(math::Color::Blue, + msgs::Convert(_emitter->Data().color_start())); + EXPECT_EQ(math::Color::Green, + msgs::Convert(_emitter->Data().color_end())); + EXPECT_DOUBLE_EQ(10.0, _emitter->Data().scale_rate().data()); + + // color range image is empty because the emitter system + // will not be able to find a file that does not exist + EXPECT_EQ("/path/to/dummy_image.png", + _emitter->Data().color_range_image().data()); + } + else + { + updateDefaultChecked = true; + + EXPECT_TRUE(_name->Data().find(std::to_string(_entity)) + != std::string::npos); + EXPECT_EQ(_name->Data(), _emitter->Data().name()); + EXPECT_EQ(msgs::ParticleEmitter_EmitterType_POINT, + _emitter->Data().type()); + EXPECT_EQ(math::Pose3d(0, 0, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), + msgs::Convert(_emitter->Data().pose())); + EXPECT_EQ(math::Vector3d(1, 1, 1), + msgs::Convert(_emitter->Data().size())); + EXPECT_DOUBLE_EQ(10.0, _emitter->Data().rate().data()); + EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration().data()); + EXPECT_FALSE(_emitter->Data().emitting().data()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime().data()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity().data()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity().data()); + EXPECT_EQ(math::Color::White, + msgs::Convert(_emitter->Data().color_start())); + EXPECT_EQ(math::Color::White, + msgs::Convert(_emitter->Data().color_end())); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().scale_rate().data()); + EXPECT_EQ("", _emitter->Data().color_range_image().data()); + } + + return true; + }); + }); + + this->server->AddSystem(testSystem.systemPtr); + this->server->Run(true, 1, false); + + EXPECT_TRUE(updateCustomChecked); + EXPECT_TRUE(updateDefaultChecked); +} diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 173fb1d422..a08269dc3d 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -472,7 +472,7 @@ TEST_F(PhysicsSystemFixture, SetFrictionCoefficient) test::Relay testSystem; testSystem.OnPostUpdate( - [&boxParams, &poses](const gazebo::UpdateInfo &_info, + [&boxParams, &poses](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { _ecm.Each( diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 7dbb63b4c1..3e5fffca4c 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -220,6 +220,57 @@ TEST_P(SceneBroadcasterTest, SceneTopic) EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene)); } +///////////////////////////////////////////////// +/// Test whether the scene topic is published only when new entities are added +TEST_P(SceneBroadcasterTest, SceneTopicSensors) +{ + // Start server + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/altimeter_with_pose.sdf"); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + EXPECT_EQ(12u, *server.EntityCount()); + + // Create requester + transport::Node node; + + std::vector sceneMsgs; + std::function collectMsgs = + [&sceneMsgs](const msgs::Scene &_msg) + { + sceneMsgs.push_back(_msg); + }; + + node.Subscribe("/world/altimeter_sensor/scene/info", collectMsgs); + + // Run server + server.Run(true, 10, false); + + // Should only have one scene even though the simulation ran multiple times + ASSERT_EQ(1u, sceneMsgs.size()); + + // Compare this scene with one from a service request + msgs::Scene &scene = sceneMsgs.front(); + + bool result{false}; + unsigned int timeout{5000}; + ignition::msgs::Scene msg; + + EXPECT_TRUE(node.Request("/world/altimeter_sensor/scene/info", + timeout, msg, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(google::protobuf::util::MessageDifferencer::Equals(msg, scene)); + + EXPECT_EQ(1, msg.model(1).link(0).sensor_size()); + EXPECT_EQ("altimeter_sensor", msg.model(1).link(0).sensor(0).name()); + EXPECT_DOUBLE_EQ(0.1, msg.model(1).link(0).sensor(0).pose().position().x()); + EXPECT_DOUBLE_EQ(0.2, msg.model(1).link(0).sensor(0).pose().position().y()); + EXPECT_DOUBLE_EQ(0.3, msg.model(1).link(0).sensor(0).pose().position().z()); +} + ///////////////////////////////////////////////// /// Test whether the scene topic is published only when new entities are added TEST_P(SceneBroadcasterTest, DeletedTopic) diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc index cd5f16b020..fc94354581 100644 --- a/test/integration/thermal_sensor_system.cc +++ b/test/integration/thermal_sensor_system.cc @@ -172,13 +172,18 @@ TEST_F(ThermalSensorTest, // verify temperature of heat source const std::string sphereVisual = "sphere_visual"; const std::string cylinderVisual = "cylinder_visual"; - EXPECT_EQ(2u, entityTemp.size()); + const std::string boxVisual = "box_visual"; + EXPECT_EQ(3u, entityTemp.size()); ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end()); ASSERT_TRUE(entityTemp.find(cylinderVisual) != entityTemp.end()); + ASSERT_TRUE(entityTemp.find(boxVisual) != entityTemp.end()); EXPECT_DOUBLE_EQ(600.0, entityTemp[sphereVisual].Kelvin()); // the user specified temp is larger than the max value representable by an // 8 bit 3 degree resolution camera - this value should be clamped EXPECT_DOUBLE_EQ(800.0, entityTemp[cylinderVisual].Kelvin()); + // the user specified temp is less than the min value of the camera - this + // value should be clamped + EXPECT_DOUBLE_EQ(-10.0, entityTemp[boxVisual].Kelvin()); // Run server server.Run(true, 35, false); @@ -203,9 +208,11 @@ TEST_F(ThermalSensorTest, std::lock_guard lock(g_mutex); unsigned int leftIdx = height * 0.5 * width; unsigned int rightIdx = leftIdx + width-1; + unsigned int middleIdx = leftIdx + (0.5 * width); unsigned int defaultResolution = 3u; unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution; unsigned int sphereTemp = g_image[rightIdx] * defaultResolution; + unsigned int boxTemp = g_image[middleIdx] * defaultResolution; // default resolution, min, max values used so we should get correct // temperature value EXPECT_EQ(600u, sphereTemp); @@ -213,6 +220,9 @@ TEST_F(ThermalSensorTest, // in the image output to: // 2^(bitDepth) - 1 * resolution = 2^8 - 1 * 3 = 765 EXPECT_EQ(765u, cylinderTemp); + // the box resolution should be clamped to the camera's default + // minimum temperature (0) + EXPECT_EQ(0u, boxTemp); } g_imageMsgs.clear(); diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 41710e0d86..9d8dd1e44d 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -989,4 +989,21 @@ TEST_F(UserCommandsTest, Physics) physicsComp = ecm->Component(worldEntity); EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); + + // Send invalid values (not > 0) and make sure they are not updated + req.set_max_step_size(0.0); + req.set_real_time_factor(0.0); + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + // Run two iterations, in the first one the PhysicsCmd component is created + // in the second one it is processed + server.Run(true, 2, false); + + // Check updated physics properties + physicsComp = ecm->Component(worldEntity); + EXPECT_DOUBLE_EQ(0.123, physicsComp->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(4.567, physicsComp->Data().RealTimeFactor()); } diff --git a/test/integration/velocity_control_system.cc b/test/integration/velocity_control_system.cc index 32e259fb38..5eb7722440 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -21,6 +21,7 @@ #include #include +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Pose.hh" @@ -126,6 +127,112 @@ class VelocityControlTest : public ::testing::TestWithParam EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; } } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestPublishLinkCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + // currently only tpe supports link velocity cmds + serverConfig.SetPhysicsEngine("ignition-physics-tpe-plugin"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle link poses + test::Relay testSystem; + + std::vector modelPoses; + std::vector linkPoses; + testSystem.OnPostUpdate([&linkPoses, &modelPoses]( + const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto modelId = _ecm.EntityByComponents( + components::Model(), + components::Name("vehicle_blue")); + EXPECT_NE(kNullEntity, modelId); + + auto modelPoseComp = _ecm.Component(modelId); + ASSERT_NE(nullptr, modelPoseComp); + + modelPoses.push_back(modelPoseComp->Data()); + + auto linkId = _ecm.EntityByComponents( + components::Link(), + components::Name("caster")); + EXPECT_NE(kNullEntity, linkId); + + auto linkPoseComp = _ecm.Component(linkId); + ASSERT_NE(nullptr, linkPoseComp); + + linkPoses.push_back(linkPoseComp->Data()); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, modelPoses.size()); + + for (const auto &pose : modelPoses) + { + EXPECT_EQ(modelPoses[0], pose); + } + for (const auto &pose : linkPoses) + { + EXPECT_EQ(linkPoses[0], pose); + } + + // Publish command and check that link moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + msgs::Twist msg; + + const double desiredLinVel = 10.5; + const double desiredAngVel = 0.2; + msgs::Set(msg.mutable_linear(), + math::Vector3d(desiredLinVel, 0, 0)); + msgs::Set(msg.mutable_angular(), + math::Vector3d(0.0, 0, desiredAngVel)); + pub.Publish(msg); + + // Give some time for message to be received + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, modelPoses.size()); + ASSERT_EQ(4000u, linkPoses.size()); + + // verify that the model is stationary + for (unsigned int i = 1001; i < modelPoses.size(); ++i) + { + EXPECT_EQ(modelPoses[0], modelPoses[i]); + } + + // verify that the link is moving in +x and rotating about its origin + for (unsigned int i = 1001; i < linkPoses.size(); ++i) + { + EXPECT_GT(linkPoses[i].Pos().X(), linkPoses[i-1].Pos().X()) << i; + EXPECT_NEAR(linkPoses[i].Pos().Y(), linkPoses[i-1].Pos().Y(), 1e-5); + EXPECT_NEAR(linkPoses[i].Pos().Z(), linkPoses[i-1].Pos().Z(), 1e-5); + EXPECT_NEAR(linkPoses[i].Rot().Euler().X(), + linkPoses[i-1].Rot().Euler().X(), 1e-5) << i; + EXPECT_NEAR(linkPoses[i].Rot().Euler().Y(), + linkPoses[i-1].Rot().Euler().Y(), 1e-5) << i; + EXPECT_GT(linkPoses[i].Rot().Euler().Z(), + linkPoses[i-1].Rot().Euler().Z()) << i; + } + } }; ///////////////////////////////////////////////// @@ -137,52 +244,11 @@ TEST_P(VelocityControlTest, PublishCmd) } ///////////////////////////////////////////////// -TEST_F(VelocityControlTest, InitialVelocity) +TEST_P(VelocityControlTest, PublishLinkCmd) { - // Start server - ServerConfig serverConfig; - serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "velocity_control.sdf")); - - Server server(serverConfig); - EXPECT_FALSE(server.Running()); - EXPECT_FALSE(*server.Running(0)); - - // Create a system that records the vehicle poses - test::Relay testSystem; - - std::vector poses; - testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, - const gazebo::EntityComponentManager &_ecm) - { - auto id = _ecm.EntityByComponents( - components::Model(), - components::Name("vehicle_green")); - EXPECT_NE(kNullEntity, id); - - auto poseComp = _ecm.Component(id); - ASSERT_NE(nullptr, poseComp); - - poses.push_back(poseComp->Data()); - }); - server.AddSystem(testSystem.systemPtr); - - // Run server and check that vehicle moved - server.Run(true, 1000, false); - EXPECT_EQ(1000u, poses.size()); - - // verify that the vehicle is moving in +x and rotating towards -y - for (unsigned int i = 1u; i < poses.size(); ++i) - { - EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()) << i; - EXPECT_LT(poses[i].Pos().Y(), poses[i-1].Pos().Y()) << i; - EXPECT_NEAR(poses[i].Pos().Z(), poses[i-1].Pos().Z(), 1e-5); - EXPECT_NEAR(poses[i].Rot().Euler().X(), - poses[i-1].Rot().Euler().X(), 1e-5) << i; - EXPECT_NEAR(poses[i].Rot().Euler().Y(), - poses[i-1].Rot().Euler().Y(), 1e-5) << i; - EXPECT_LT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; - } + TestPublishLinkCmd( + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/velocity_control.sdf", + "/model/vehicle_blue/link/caster/cmd_vel"); } // Run multiple times diff --git a/test/integration/wheel_slip.cc b/test/integration/wheel_slip.cc index 7480197a35..ebc26042fa 100644 --- a/test/integration/wheel_slip.cc +++ b/test/integration/wheel_slip.cc @@ -554,12 +554,15 @@ TEST_F(WheelSlipTest, TricyclesUphill) server.AddSystem(testSlipSystem.systemPtr); server.Run(true, 2000, false); + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ // compute expected slip // normal force as passed to Wheel Slip in test world const double wheelNormalForce = 32; const double mass = 14.5; const double forceRatio = (mass/2) * std::abs(gravity->Data().X()) / wheelNormalForce; +#endif const double noSlipLinearSpeed = wheelRadius * angularSpeed; auto wheelRearLeftVelocity = @@ -590,6 +593,9 @@ TEST_F(WheelSlipTest, TricyclesUphill) EXPECT_NEAR(angularSpeed, wheelRearLeftVelocity->Data()[0], 3e-3); EXPECT_NEAR(angularSpeed, wheelRearRightVelocity->Data()[0], 3e-3); + // Slip works on DART>=6.10, which isn't available on Ubuntu Focal +#ifndef __linux__ EXPECT_NEAR(noSlipLinearSpeed - worldVelTrisphere1->Data()[0], noSlipLinearSpeed * forceRatio, 5e-3); +#endif } diff --git a/test/performance/ign_perf.py b/test/performance/ign_perf.py index 3856ef9f8c..ae0921b7ed 100755 --- a/test/performance/ign_perf.py +++ b/test/performance/ign_perf.py @@ -19,7 +19,7 @@ def read_data(filename): entries.append([float(r) for r in row]) return (header, np.array(entries)) -def compute_rtfs(data): +def compute_rtfs(real_time, sim_time): # Compute time deltas real_dt = np.diff(real_time) sim_dt = np.diff(sim_time) @@ -40,7 +40,7 @@ def compute_rtfs(data): real_time = data[:,0] + 1e-9 * data[:,1] sim_time = data[:,2] + 1e-9 * data[:,3] - rtfs = compute_rtfs(data) + rtfs = compute_rtfs(real_time, sim_time) if args.summarize: iters = len(data) diff --git a/test/worlds/altimeter_with_pose.sdf b/test/worlds/altimeter_with_pose.sdf new file mode 100644 index 0000000000..fda866b333 --- /dev/null +++ b/test/worlds/altimeter_with_pose.sdf @@ -0,0 +1,91 @@ + + + + + 0.001 + 1.0 + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 0.1 0.2 0.3 0 0 0 + 1 + 30 + true + + + + + link + 2 + + + + + diff --git a/test/worlds/odometry_publisher.sdf b/test/worlds/odometry_publisher.sdf new file mode 100644 index 0000000000..e572a8cc88 --- /dev/null +++ b/test/worlds/odometry_publisher.sdf @@ -0,0 +1,230 @@ + + + + + + 0.001 + 1.0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + + + + + + diff --git a/test/worlds/odometry_publisher_custom.sdf b/test/worlds/odometry_publisher_custom.sdf new file mode 100644 index 0000000000..7ef913aea1 --- /dev/null +++ b/test/worlds/odometry_publisher_custom.sdf @@ -0,0 +1,234 @@ + + + + + + 0.001 + 1.0 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0 0 0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + + -0.957138 -0 -0.125 0 -0 0 + + 1 + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + + + + + 0.2 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.2 + + + + + + + chassis + left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + caster + + + + 50 + /model/bar/odom + odomCustom + baseCustom + + + + + + diff --git a/test/worlds/particle_emitter.sdf b/test/worlds/particle_emitter.sdf index 1da40a4919..12912e5f5b 100644 --- a/test/worlds/particle_emitter.sdf +++ b/test/worlds/particle_emitter.sdf @@ -121,6 +121,7 @@ 0 1 0 10 /path/to/dummy_image.png + 0.01 diff --git a/test/worlds/particle_emitter2.sdf b/test/worlds/particle_emitter2.sdf new file mode 100644 index 0000000000..7a8781512b --- /dev/null +++ b/test/worlds/particle_emitter2.sdf @@ -0,0 +1,150 @@ + + + + + + 0.001 + 1.0 + + + + + + + + + + + 3D View + false + false + 0 + + + + + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -1 0 1 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + + + + true + true + true + + + + + + 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 + + + + + + + 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 0 0 0 + true + + + + + 1 1 1 + + + + + 0 1 0 0 0 0 + 2 2 2 + 5 + 1 + true + 3 3 3 + 2 + + 10 + 20 + 0 0 1 + 0 1 0 + 10 + /path/to/dummy_image.png + + + + + + + + 5 5 0 0 0 0 + true + + + + + 1 1 1 + + + + + + + + + + + + diff --git a/test/worlds/thermal_invalid.sdf b/test/worlds/thermal_invalid.sdf index 89c621b9c9..11cc94c6e0 100644 --- a/test/worlds/thermal_invalid.sdf +++ b/test/worlds/thermal_invalid.sdf @@ -38,7 +38,7 @@ true` - 0 0.5 0.5 0 0 0 + 0 0.85 0.5 0 0 0 @@ -70,7 +70,7 @@ true` - 0 -0.5 0.5 0 0 0 + 0 -0.85 0.5 0 0 0 @@ -102,6 +102,38 @@ + + true` + -1 0.0 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + -10.0 + + + + + 1.0 0 0.5 0.0 0.0 3.14 diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf index 71c30486f9..d3659d4884 100644 --- a/test/worlds/velocity_control.sdf +++ b/test/worlds/velocity_control.sdf @@ -310,6 +310,7 @@ + caster