diff --git a/.github/ci/after_make.sh b/.github/ci/after_make.sh index eb81ef7985..8e619ed869 100644 --- a/.github/ci/after_make.sh +++ b/.github/ci/after_make.sh @@ -5,6 +5,7 @@ set -e # Install (needed for some tests) make install +export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib # For ign-tools export IGN_CONFIG_PATH=/usr/local/share/ignition diff --git a/Changelog.md b/Changelog.md index 60ee6dc27d..6e030ad5e1 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,98 @@ ## Ignition Gazebo 4.x +### Ignition Gazebo 4.5.0 (2020-02-17) + +1. Added particle system. + * [Pull Request 516](https://github.com/ignitionrobotics/ign-gazebo/pull/516) + +1. Add Light Usercommand and include Light parameters in the componentInspector + * [Pull Request 482](https://github.com/ignitionrobotics/ign-gazebo/pull/482) + +1. Added link to HW-accelerated video recording. + * [Pull Request 627](https://github.com/ignitionrobotics/ign-gazebo/pull/627) + +1. Fix EntityComponentManager race condition. + * [Pull Request 601](https://github.com/ignitionrobotics/ign-gazebo/pull/601) + +1. Add SDF topic validity check. + * [Pull Request 632](https://github.com/ignitionrobotics/ign-gazebo/pull/632) + +1. Add JointTrajectoryController system plugin. + * [Pull Request 473](https://github.com/ignitionrobotics/ign-gazebo/pull/473) + +### Ignition Gazebo 4.4.0 (2020-02-10) + +1. Added issue and PR templates + * [Pull Request 613](https://github.com/ignitionrobotics/ign-gazebo/pull/613) + +1. Fix segfault in SetRemovedComponentsMsgs method + * [Pull Request 495](https://github.com/ignitionrobotics/ign-gazebo/pull/495) + +1. Make topics configurable for joint controllers + * [Pull Request 584](https://github.com/ignitionrobotics/ign-gazebo/pull/584) + +1. Add about dialog + * [Pull Request 609](https://github.com/ignitionrobotics/ign-gazebo/pull/609) + +1. Add thermal sensor system for configuring thermal camera properties + * [Pull Request 614](https://github.com/ignitionrobotics/ign-gazebo/pull/614) + +### Ignition Gazebo 4.3.0 (2020-02-02) + +1. Non-blocking paths request. + * [Pull Request 555](https://github.com/ignitionrobotics/ign-gazebo/pull/555) + +1. Parallelize State call in ECM. + * [Pull Request 451](https://github.com/ignitionrobotics/ign-gazebo/pull/451) + +1. Allow to create light with the create service. + * [Pull Request 513](https://github.com/ignitionrobotics/ign-gazebo/pull/513) + +1. Added size to ground_plane in examples. + * [Pull Request 573](https://github.com/ignitionrobotics/ign-gazebo/pull/573) + +1. Fix finding PBR materials. + * [Pull Request 575](https://github.com/ignitionrobotics/ign-gazebo/pull/575) + +1. Publish all periodic change components in Scene Broadcaster. + * [Pull Request 544](https://github.com/ignitionrobotics/ign-gazebo/pull/544) + +1. Backport state update changes from pull request [#486](https://github.com/ignitionrobotics/ign-gazebo/pull/486). + * [Pull Request 583](https://github.com/ignitionrobotics/ign-gazebo/pull/583) + +1. Fix code_check errors. + * [Pull Request 582](https://github.com/ignitionrobotics/ign-gazebo/pull/582) + +1. Visualize collisions. + * [Pull Request 531](https://github.com/ignitionrobotics/ign-gazebo/pull/531) + +1. Remove playback SDF param in Dome. + * [Pull Request 570](https://github.com/ignitionrobotics/ign-gazebo/pull/570) + +1. Tutorial on migrating SDF files from Gazebo classic. + * [Pull Request 400](https://github.com/ignitionrobotics/ign-gazebo/pull/400) + +1. World Exporter. + * [Pull Request 474](https://github.com/ignitionrobotics/ign-gazebo/pull/474) + +1. Model Creation tutorial using services. + * [Pull Request 530](https://github.com/ignitionrobotics/ign-gazebo/pull/530) + +1. Fix topLevelModel Method. + * [Pull Request 600](https://github.com/ignitionrobotics/ign-gazebo/pull/600) + +1. Add heat signature option to thermal system. + * [Pull Request 498](https://github.com/ignitionrobotics/ign-gazebo/pull/498) + +1. Add service and GUI to configure physics parameters (step size and RTF). + * [Pull Request 536](https://github.com/ignitionrobotics/ign-gazebo/pull/536) + +1. Refactor UNIT_Server_TEST. + * [Pull Request 594](https://github.com/ignitionrobotics/ign-gazebo/pull/594) + +1. Use Ignition GUI render event. + * [Pull Request 598](https://github.com/ignitionrobotics/ign-gazebo/pull/598) + ### Ignition Gazebo 4.2.0 (2020-01-13) 1. Automatically load a subset of world plugins. diff --git a/Migration.md b/Migration.md index 7f52fe493e..26ebc1896d 100644 --- a/Migration.md +++ b/Migration.md @@ -48,6 +48,13 @@ in SDF by setting the `` SDF element. std::string(const gazebo::Entity &, const sdf::Sensor &, const std::string &)>)` +* Log playback using `` SDF parameter is removed. Use --playback command + line argument instead. + +* `rendering::SceneManager` + * **Deprecated**: `Entity EntityFromNode(const rendering::NodePtr &_node) const;` + * **Replacement**: `Entity entity = std::get(visual->UserData("gazebo-entity"));` + ## Ignition Gazebo 2.x to 3.x * Use ign-rendering3, ign-sensors3 and ign-gui3. diff --git a/examples/scripts/distributed/secondary.sdf b/examples/scripts/distributed/secondary.sdf index f9dc09ed5c..13312dc93f 100644 --- a/examples/scripts/distributed/secondary.sdf +++ b/examples/scripts/distributed/secondary.sdf @@ -28,6 +28,7 @@ 0 0 1 + 100 100 diff --git a/examples/scripts/distributed/standalone.sdf b/examples/scripts/distributed/standalone.sdf index 94a6d74396..1b95237900 100644 --- a/examples/scripts/distributed/standalone.sdf +++ b/examples/scripts/distributed/standalone.sdf @@ -108,6 +108,7 @@ 0 0 1 + 100 100 diff --git a/examples/standalone/entity_creation/CMakeLists.txt b/examples/standalone/entity_creation/CMakeLists.txt new file mode 100644 index 0000000000..e82d5965c8 --- /dev/null +++ b/examples/standalone/entity_creation/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) + +add_executable(entity_creation entity_creation.cc) +target_link_libraries(entity_creation + ignition-transport${IGN_TRANSPORT_VER}::core) diff --git a/examples/standalone/entity_creation/README.md b/examples/standalone/entity_creation/README.md new file mode 100644 index 0000000000..e9a4ffcc18 --- /dev/null +++ b/examples/standalone/entity_creation/README.md @@ -0,0 +1,25 @@ +# Examples using the create service + +## Build + +``` +mkdir build +cd build +cmake .. +make +``` + +## Run + +This example only works if the world is called `empty`. Start an empty world with: + +``` +ign gazebo empty.sdf +``` + +Then run the create program to spawn entities into the world: + +``` +cd build +./entity_creation +``` diff --git a/examples/standalone/entity_creation/entity_creation.cc b/examples/standalone/entity_creation/entity_creation.cc new file mode 100644 index 0000000000..6184f1beef --- /dev/null +++ b/examples/standalone/entity_creation/entity_creation.cc @@ -0,0 +1,186 @@ +/* + * 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 + +// Create a transport node. +ignition::transport::Node node; + +// timeout used for services +constexpr unsigned int timeout = 5000; + +void createLight() +{ + bool result; + ignition::msgs::Boolean rep; +//! [create light] + ignition::msgs::EntityFactory entityFactoryRequest; + + entityFactoryRequest.mutable_light()->set_name("point"); + entityFactoryRequest.mutable_light()->set_range(4); + entityFactoryRequest.mutable_light()->set_attenuation_linear(0.5); + entityFactoryRequest.mutable_light()->set_attenuation_constant(0.2); + entityFactoryRequest.mutable_light()->set_attenuation_quadratic(0.01); + entityFactoryRequest.mutable_light()->set_cast_shadows(false); + entityFactoryRequest.mutable_light()->set_type(ignition::msgs::Light::POINT); + ignition::msgs::Set( + entityFactoryRequest.mutable_light()->mutable_direction(), + ignition::math::Vector3d(0.5, 0.2, -0.9)); + ignition::msgs::Set(entityFactoryRequest.mutable_light()->mutable_pose(), + ignition::math::Pose3d(0.0, 0, 3.0, 0.0, 0.0, 0.0)); +//! [create light] + +//! [call service create] + bool executedEntityFactory = node.Request("/world/empty/create", + entityFactoryRequest, timeout, rep, result); + if (executedEntityFactory) + { + if (result) + std::cout << "Light was created : [" << rep.data() << "]" << std::endl; + else + { + std::cout << "Service call failed" << std::endl; + return; + } + } + else + std::cerr << "Service call timed out" << std::endl; +//! [call service create] +} + +void createEntityFromStr(const std::string modelStr) +{ +//! [call service create sphere] + bool result; + ignition::msgs::EntityFactory req; + ignition::msgs::Boolean res; + req.set_sdf(modelStr); + + bool executed = node.Request("/world/empty/create", + req, timeout, res, result); + if (executed) + { + if (result) + std::cout << "Sphere was created : [" << res.data() << "]" << std::endl; + else + { + std::cout << "Service call failed" << std::endl; + return; + } + } + else + std::cerr << "Service call timed out" << std::endl; +//! [call service create sphere] +} + +////////////////////////////////////////////////// +std::string generateLightStr( + const std::string light_type, const std::string name, + const bool cast_shadows, const ignition::math::Pose3d pose, + const ignition::math::Color diffuse, + const ignition::math::Color specular, + const double attRange, const double attConstant, + const double attLinear, const double attQuadratic, + const ignition::math::Vector3d direction, + const double spot_inner_angle, + const double spot_outer_angle, + const double spot_falloff +) +{ +//! [create light str] + std::string lightStr = std::string("") + + " " + + "" + std::to_string(cast_shadows) + "" + + "" + + std::to_string(pose.Pos().X()) + " " + + std::to_string(pose.Pos().Y()) + " " + + std::to_string(pose.Pos().Z()) + " " + + std::to_string(pose.Rot().Roll()) + " " + + std::to_string(pose.Rot().Pitch()) + " " + + std::to_string(pose.Rot().Yaw()) + + "" + + "" + + std::to_string(diffuse.R()) + " " + + std::to_string(diffuse.G()) + " " + + std::to_string(diffuse.B()) + " " + + std::to_string(diffuse.A()) + + "" + + "" + + std::to_string(specular.R()) + " " + + std::to_string(specular.G()) + " " + + std::to_string(specular.B()) + " " + + std::to_string(specular.A()) + + "" + + "" + + "" + std::to_string(attRange) + "" + + "" + std::to_string(attConstant) + "" + + "" + std::to_string(attLinear) + "" + + "" + std::to_string(attQuadratic) + "" + + "" + + "" + + std::to_string(direction.X()) + " " + + std::to_string(direction.Y()) + " " + + std::to_string(direction.Z()) + + "" + + "" + + "" + std::to_string(spot_inner_angle) + "" + + "" + std::to_string(spot_outer_angle) + "" + + "" + std::to_string(spot_falloff) + "" + + "" + + ""; +//! [create light str] + return lightStr; +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ +//! [create sphere] + auto sphereStr = R"( + + + + + 0 0 0.5 0 0 0 + + 1 + + + 1 + + + + )"; +//! [create sphere] + + createEntityFromStr(sphereStr); + + createEntityFromStr( + generateLightStr("spot", "spot_light", false, + ignition::math::Pose3d(0, 0, 4, 0, 0, 0), + ignition::math::Color(0, 0, 1.0, 1.0), + ignition::math::Color(0, 0, 1.0, 1.0), + 1.0, 0.2, 0.2, 0.001, + ignition::math::Vector3d(0.5, 0.2, -0.9), + 0.15, 0.45, 1.0)); + + createLight(); +} diff --git a/examples/standalone/light_control/CMakeLists.txt b/examples/standalone/light_control/CMakeLists.txt new file mode 100644 index 0000000000..cad2d0d7ac --- /dev/null +++ b/examples/standalone/light_control/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) + +find_package(ignition-gazebo5 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + +add_executable(light_control light_control.cc) +target_link_libraries(light_control + ignition-transport${IGN_TRANSPORT_VER}::core + ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) diff --git a/examples/standalone/light_control/light_control.cc b/examples/standalone/light_control/light_control.cc new file mode 100644 index 0000000000..4f499787c2 --- /dev/null +++ b/examples/standalone/light_control/light_control.cc @@ -0,0 +1,179 @@ +/* + * Copyright (C) 2020 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 + +using namespace std::chrono_literals; + +// Create a transport node. +ignition::transport::Node node; + +bool result; +// timeout used for services +constexpr unsigned int timeout = 5000; + +std::mt19937 twister(std::time(0)); +std::uniform_real_distribution distr(0, 1000); +constexpr double epsilon = 0.1; + +float directionX = 0.5; +float directionY = 0.2; +float directionZ = -0.9; + +void createLight() +{ + ignition::msgs::Boolean rep; +//! [create light] + ignition::msgs::EntityFactory entityFactoryRequest; + + entityFactoryRequest.mutable_light()->set_name("point"); + entityFactoryRequest.mutable_light()->set_range(4); + entityFactoryRequest.mutable_light()->set_attenuation_linear(0.5); + entityFactoryRequest.mutable_light()->set_attenuation_constant(0.2); + entityFactoryRequest.mutable_light()->set_attenuation_quadratic(0.01); + entityFactoryRequest.mutable_light()->set_cast_shadows(false); + entityFactoryRequest.mutable_light()->set_type(ignition::msgs::Light::POINT); + ignition::msgs::Set( + entityFactoryRequest.mutable_light()->mutable_direction(), + ignition::math::Vector3d(directionX, directionY, directionZ)); + ignition::msgs::Set(entityFactoryRequest.mutable_light()->mutable_pose(), + ignition::math::Pose3d(0.0, 0, 3.0, 0.0, 0.0, 0.0)); +//! [create light] + + bool executedEntityFactory = node.Request("/world/empty/create", + entityFactoryRequest, timeout, rep, result); + if (executedEntityFactory) + { + if (result) + std::cout << "Light was created : [" << rep.data() << "]" << std::endl; + else + { + std::cout << "Service call failed" << std::endl; + return; + } + } + else + std::cerr << "Service call timed out" << std::endl; +} + +void createSphere() +{ + auto modelStr = R"( + + + + + 0 0 0.5 0 0 0 + + 1 + + + 1 + + + + )"; + + ignition::msgs::EntityFactory req; + ignition::msgs::Boolean res; + req.set_sdf(modelStr); + + bool executed = node.Request("/world/empty/create", + req, timeout, res, result); + if (executed) + { + if (result) + std::cout << "Sphere was created : [" << res.data() << "]" << std::endl; + else + { + std::cout << "Service call failed" << std::endl; + return; + } + } + else + std::cerr << "Service call timed out" << std::endl; +} + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ignition::msgs::Boolean rep; + ignition::msgs::Light lightRequest; + auto lightConfigService = "/world/empty/light_config"; + + createSphere(); + createLight(); + + while (1) + { + float r, g, b; + double m = 0; +//! [random numbers] + while (m < epsilon) + { + r = distr(twister); + g = distr(twister); + b = distr(twister); + m = std::sqrt(r*r + b*b + g*g); + } + r /= m; + b /= m; + b /= m; +//! [random numbers] + +//! [modify light] + lightRequest.set_name("point"); + lightRequest.set_range(4); + lightRequest.set_attenuation_linear(0.5); + lightRequest.set_attenuation_constant(0.2); + lightRequest.set_attenuation_quadratic(0.01); + lightRequest.set_cast_shadows(false); + lightRequest.set_type(ignition::msgs::Light::POINT); + // direction field only affects spot / directional lights + ignition::msgs::Set(lightRequest.mutable_direction(), + ignition::math::Vector3d(directionX, directionY, directionZ)); + ignition::msgs::Set(lightRequest.mutable_pose(), + ignition::math::Pose3d(0.0, -1.5, 3.0, 0.0, 0.0, 0.0)); + ignition::msgs::Set(lightRequest.mutable_diffuse(), + ignition::math::Color(r, g, b, 1)); + ignition::msgs::Set(lightRequest.mutable_specular(), + ignition::math::Color(r, g, b, 1)); +//! [modify light] + bool executed = node.Request(lightConfigService, lightRequest, timeout, + rep, result); + std::cout << "Service called: [" << r << ", " << g << ", " << b << "]" + << std::endl; + + if (executed) + { + if (result) + std::cout << "Response: [" << rep.data() << "]" << std::endl; + else + std::cout << "Service call failed" << std::endl; + } + else + std::cerr << "Service call timed out" << std::endl; + + std::this_thread::sleep_for(1s); + } +} diff --git a/examples/worlds/3k_shapes.sdf b/examples/worlds/3k_shapes.sdf index 8f6597e94c..e0f014f1a1 100644 --- a/examples/worlds/3k_shapes.sdf +++ b/examples/worlds/3k_shapes.sdf @@ -130,6 +130,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/actor.sdf b/examples/worlds/actor.sdf index da64676015..346c511b26 100644 --- a/examples/worlds/actor.sdf +++ b/examples/worlds/actor.sdf @@ -126,6 +126,7 @@ 0.0 0.0 1 + 100 100 diff --git a/examples/worlds/actors_population.sdf.erb b/examples/worlds/actors_population.sdf.erb index 0d62684efe..98bc2a2ad0 100644 --- a/examples/worlds/actors_population.sdf.erb +++ b/examples/worlds/actors_population.sdf.erb @@ -118,6 +118,7 @@ 0.0 0.0 1 + 100 100 diff --git a/examples/worlds/apply_joint_force.sdf b/examples/worlds/apply_joint_force.sdf index 2fc15ab1f3..1825eb7a4b 100644 --- a/examples/worlds/apply_joint_force.sdf +++ b/examples/worlds/apply_joint_force.sdf @@ -41,6 +41,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/breadcrumbs.sdf b/examples/worlds/breadcrumbs.sdf index 69033f1ee7..b3ade84636 100644 --- a/examples/worlds/breadcrumbs.sdf +++ b/examples/worlds/breadcrumbs.sdf @@ -55,6 +55,7 @@ 0 0 1 + 100 100 @@ -413,4 +414,3 @@ - diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf index 608fa57f7e..b79e76b81b 100644 --- a/examples/worlds/camera_sensor.sdf +++ b/examples/worlds/camera_sensor.sdf @@ -123,6 +123,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/camera_video_record_dbl_pendulum.sdf b/examples/worlds/camera_video_record_dbl_pendulum.sdf index ececf4d6fc..abd7817c68 100644 --- a/examples/worlds/camera_video_record_dbl_pendulum.sdf +++ b/examples/worlds/camera_video_record_dbl_pendulum.sdf @@ -54,6 +54,7 @@ 0 0 1 + 100 100 @@ -320,4 +321,3 @@ - diff --git a/examples/worlds/collada_world_exporter.sdf b/examples/worlds/collada_world_exporter.sdf new file mode 100644 index 0000000000..cd4b089ddd --- /dev/null +++ b/examples/worlds/collada_world_exporter.sdf @@ -0,0 +1,180 @@ + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 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.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + diff --git a/examples/worlds/contact_sensor.sdf b/examples/worlds/contact_sensor.sdf index e9baaaad1e..6e84f1c29c 100644 --- a/examples/worlds/contact_sensor.sdf +++ b/examples/worlds/contact_sensor.sdf @@ -52,6 +52,7 @@ Run the following to print out contacts, 0 0 1 + 100 100 diff --git a/examples/worlds/debug_shapes.sdf b/examples/worlds/debug_shapes.sdf index 1e83a2eb19..10ed07430f 100644 --- a/examples/worlds/debug_shapes.sdf +++ b/examples/worlds/debug_shapes.sdf @@ -43,6 +43,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/default.sdf b/examples/worlds/default.sdf index f359666e1f..a963e7e709 100644 --- a/examples/worlds/default.sdf +++ b/examples/worlds/default.sdf @@ -39,6 +39,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/depth_camera_sensor.sdf b/examples/worlds/depth_camera_sensor.sdf index 687a3faa13..ce693fe1b0 100644 --- a/examples/worlds/depth_camera_sensor.sdf +++ b/examples/worlds/depth_camera_sensor.sdf @@ -113,6 +113,7 @@ 20 20 0.1 diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index b5cb606e1e..c88799cda7 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -56,6 +56,7 @@ 0 0 1 + 100 100 @@ -482,4 +483,3 @@ - diff --git a/examples/worlds/diff_drive.sdf b/examples/worlds/diff_drive.sdf index e1e711ec77..647509944d 100644 --- a/examples/worlds/diff_drive.sdf +++ b/examples/worlds/diff_drive.sdf @@ -56,6 +56,7 @@ 0 0 1 + 100 100 @@ -436,4 +437,3 @@ - diff --git a/examples/worlds/diff_drive_skid.sdf b/examples/worlds/diff_drive_skid.sdf index dad1d68639..4edf93497d 100644 --- a/examples/worlds/diff_drive_skid.sdf +++ b/examples/worlds/diff_drive_skid.sdf @@ -52,6 +52,7 @@ 0 0 1 + 100 100 @@ -351,4 +352,3 @@ - diff --git a/examples/worlds/empty.sdf b/examples/worlds/empty.sdf index b32a9ea4d8..f52b21c213 100644 --- a/examples/worlds/empty.sdf +++ b/examples/worlds/empty.sdf @@ -103,6 +103,7 @@ ign service -s /world/empty/create \ 0 0 1 + 100 100 diff --git a/examples/worlds/fuel.sdf b/examples/worlds/fuel.sdf index 2f8ae1e01f..a8c61e0b5b 100644 --- a/examples/worlds/fuel.sdf +++ b/examples/worlds/fuel.sdf @@ -34,6 +34,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/fuel_textured_mesh.sdf b/examples/worlds/fuel_textured_mesh.sdf index 2818448d44..7c2a39f95a 100644 --- a/examples/worlds/fuel_textured_mesh.sdf +++ b/examples/worlds/fuel_textured_mesh.sdf @@ -203,6 +203,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/gpu_lidar_retro_values_sensor.sdf b/examples/worlds/gpu_lidar_retro_values_sensor.sdf new file mode 100644 index 0000000000..f2d7a27c76 --- /dev/null +++ b/examples/worlds/gpu_lidar_retro_values_sensor.sdf @@ -0,0 +1,214 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + 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 + + + + + + 20 20 0.1 + + + + + + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 -1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 1 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + 500 + + + 1 1 1 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 4 0 0.5 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 + + + + + + lidar + 10 + + + + 640 + 1 + -1.396263 + 1.396263 + + + 16 + 1 + -0.261799 + 0.261799 + + + + 0.08 + 10.0 + 0.01 + + + 1 + true + + + + true + + + + + diff --git a/examples/worlds/gpu_lidar_sensor.sdf b/examples/worlds/gpu_lidar_sensor.sdf index 286a150752..07020dda9b 100644 --- a/examples/worlds/gpu_lidar_sensor.sdf +++ b/examples/worlds/gpu_lidar_sensor.sdf @@ -44,6 +44,7 @@ 20 20 0.1 diff --git a/examples/worlds/grid.sdf b/examples/worlds/grid.sdf index 1a383a0cc9..b775544573 100644 --- a/examples/worlds/grid.sdf +++ b/examples/worlds/grid.sdf @@ -123,6 +123,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/import_mesh.sdf b/examples/worlds/import_mesh.sdf index 0015bd15a7..1309625e2d 100644 --- a/examples/worlds/import_mesh.sdf +++ b/examples/worlds/import_mesh.sdf @@ -116,6 +116,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index 6d2ad62e83..b2bd104509 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -4,9 +4,9 @@ Try sending joint position commands: - ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: -1.0" + ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: -1.0" - ign topic -t "/model/joint_position_controller_demo/joint/j1/0/cmd_pos" -m ignition.msgs.Double -p "data: 1.0" + ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: 1.0" --> @@ -45,7 +45,7 @@ 72 121 1 - + floating @@ -156,6 +156,7 @@ filename="ignition-gazebo-joint-position-controller-system" name="ignition::gazebo::systems::JointPositionController"> j1 + rotor_cmd 1 0.1 0.01 diff --git a/examples/worlds/joint_trajectory_controller.sdf b/examples/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..29b68f69a5 --- /dev/null +++ b/examples/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,722 @@ + + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 1 0 0 0 0 3.1416 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + -0.1 0 0 0 1.5708 0 + true + + + + + 0 0 1 + 5 5 + + + + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + + + + + + + + 0 0 0.25 -2.3561945 0 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + RR_position_control_joint1 + 0.7854 + 20 + 0.4 + 1.0 + -1 + 1 + -20 + 20 + + RR_position_control_joint2 + -1.5708 + 10 + 0.2 + 0.5 + -1 + 1 + -10 + 10 + + + + + + + 0 -0.5 -0.25 0 0 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + + + 0 0.5 -0.25 -0.7854 0 0 + + + world + RR_effort_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0 0.5 1 + 0 0 0.8 1 + 0 0 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_effort_control_link0 + RR_effort_control_link1 + + 1 0 0 + + 0.02 + + + + + 0 0 0.1 0 0 0 + RR_effort_control_link1 + RR_effort_control_link2 + + 1 0 0 + + 0.01 + + + + + + + custom_topic_effort_control + + + + + diff --git a/examples/worlds/lift_drag.sdf b/examples/worlds/lift_drag.sdf index 3589ce701a..d9c1f73bec 100644 --- a/examples/worlds/lift_drag.sdf +++ b/examples/worlds/lift_drag.sdf @@ -49,6 +49,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/lift_drag_battery.sdf b/examples/worlds/lift_drag_battery.sdf index 813a8a38be..afe4f8dbb9 100644 --- a/examples/worlds/lift_drag_battery.sdf +++ b/examples/worlds/lift_drag_battery.sdf @@ -52,6 +52,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/lights.sdf b/examples/worlds/lights.sdf index 46b4d6a657..f1910f0ac7 100644 --- a/examples/worlds/lights.sdf +++ b/examples/worlds/lights.sdf @@ -81,6 +81,7 @@ 0.0 0.0 1 + 100 100 diff --git a/examples/worlds/log_record_dbl_pendulum.sdf b/examples/worlds/log_record_dbl_pendulum.sdf index fdb457d22c..5d09ef9fdd 100644 --- a/examples/worlds/log_record_dbl_pendulum.sdf +++ b/examples/worlds/log_record_dbl_pendulum.sdf @@ -58,6 +58,7 @@ 0 0 1 + 100 100 @@ -279,4 +280,3 @@ - diff --git a/examples/worlds/log_record_resources.sdf b/examples/worlds/log_record_resources.sdf index d8ada02852..222181127e 100644 --- a/examples/worlds/log_record_resources.sdf +++ b/examples/worlds/log_record_resources.sdf @@ -66,6 +66,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/log_record_shapes.sdf b/examples/worlds/log_record_shapes.sdf index 5142d1ddeb..41efbe1c66 100644 --- a/examples/worlds/log_record_shapes.sdf +++ b/examples/worlds/log_record_shapes.sdf @@ -59,6 +59,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/logical_audio_sensor_plugin.sdf b/examples/worlds/logical_audio_sensor_plugin.sdf index 9b93d11f99..a86a0656b0 100644 --- a/examples/worlds/logical_audio_sensor_plugin.sdf +++ b/examples/worlds/logical_audio_sensor_plugin.sdf @@ -71,6 +71,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/logical_camera_sensor.sdf b/examples/worlds/logical_camera_sensor.sdf index 3952828079..14d596c8e2 100644 --- a/examples/worlds/logical_camera_sensor.sdf +++ b/examples/worlds/logical_camera_sensor.sdf @@ -51,6 +51,7 @@ 20 20 0.1 diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index bdb0b83514..57e5bed01f 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -67,6 +67,7 @@ You can use the velocity controller and command linear velocity and yaw angular 0 0 1 + 100 100 @@ -402,4 +403,3 @@ You can use the velocity controller and command linear velocity and yaw angular - diff --git a/examples/worlds/nested_model.sdf b/examples/worlds/nested_model.sdf index ca705c7825..34310a6361 100644 --- a/examples/worlds/nested_model.sdf +++ b/examples/worlds/nested_model.sdf @@ -45,6 +45,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/particle_emitter.sdf b/examples/worlds/particle_emitter.sdf new file mode 100644 index 0000000000..58fb36c3be --- /dev/null +++ b/examples/worlds/particle_emitter.sdf @@ -0,0 +1,90 @@ + + + + + + + + + 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 + + + + + + + https://fuel.ignitionrobotics.org/1.0/caguero/models/smoke_generator + + + + + + diff --git a/examples/worlds/performer_detector.sdf b/examples/worlds/performer_detector.sdf index bee3525430..5350526c03 100644 --- a/examples/worlds/performer_detector.sdf +++ b/examples/worlds/performer_detector.sdf @@ -53,6 +53,7 @@ 0 0 1 + 100 100 @@ -426,4 +427,3 @@ - diff --git a/examples/worlds/plane_propeller_demo.sdf b/examples/worlds/plane_propeller_demo.sdf index 1827a0c2c4..6de0ef7e92 100644 --- a/examples/worlds/plane_propeller_demo.sdf +++ b/examples/worlds/plane_propeller_demo.sdf @@ -109,6 +109,7 @@ 0 0 1 + 100000 100 diff --git a/examples/worlds/pose_publisher.sdf b/examples/worlds/pose_publisher.sdf index a3a927c68d..2c9947512e 100644 --- a/examples/worlds/pose_publisher.sdf +++ b/examples/worlds/pose_publisher.sdf @@ -47,6 +47,7 @@ 0 0 1 + 100 100 @@ -277,4 +278,3 @@ - diff --git a/examples/worlds/quadcopter.sdf b/examples/worlds/quadcopter.sdf index 7126135016..224a11cda9 100644 --- a/examples/worlds/quadcopter.sdf +++ b/examples/worlds/quadcopter.sdf @@ -56,6 +56,7 @@ 0 0 1 + 100 100 @@ -162,4 +163,3 @@ - diff --git a/examples/worlds/rolling_shapes.sdf b/examples/worlds/rolling_shapes.sdf index ac35e266ab..6c7d287631 100644 --- a/examples/worlds/rolling_shapes.sdf +++ b/examples/worlds/rolling_shapes.sdf @@ -43,6 +43,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 967511ebe0..48af629339 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -65,6 +65,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index 8a9b0b2f7a..7bf8790dc9 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -171,6 +171,7 @@ 20 20 0.1 diff --git a/examples/worlds/shapes.sdf b/examples/worlds/shapes.sdf index 5dec565b81..b43dbab638 100644 --- a/examples/worlds/shapes.sdf +++ b/examples/worlds/shapes.sdf @@ -34,6 +34,7 @@ Try moving a model: 0 0 1 + 100 100 diff --git a/examples/worlds/shapes_bitmask.sdf b/examples/worlds/shapes_bitmask.sdf index 4aca31166c..342ccb99aa 100644 --- a/examples/worlds/shapes_bitmask.sdf +++ b/examples/worlds/shapes_bitmask.sdf @@ -55,6 +55,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/shapes_population.sdf.erb b/examples/worlds/shapes_population.sdf.erb index 2f6bcba546..b65cc9c3ce 100644 --- a/examples/worlds/shapes_population.sdf.erb +++ b/examples/worlds/shapes_population.sdf.erb @@ -130,6 +130,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index c8498d3cc2..e21215537a 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -1,6 +1,6 @@ @@ -104,6 +104,18 @@ thermal_camera false + + + Thermal camera 8 Bit + floating + 350 + 315 + 500 + + thermal_camera_8bit/image + false + + @@ -121,7 +133,14 @@ - 310 + 300 + + 0.1 @@ -131,6 +150,7 @@ 20 20 0.1 @@ -157,7 +177,7 @@ - 0 0 0.5 0 0 0 + -1 1 0.5 0 0 0 @@ -192,99 +212,53 @@ - 200.0 + 285.0 - - 0 1.5 0.5 0 0 0 - - - - 3 - 0 - 0 - 3 - 0 - 3 - - 3.0 - - - - - 0.5 - - - - - - - - 0.5 - - - - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 - - - 600.0 - - - - - - - 0 -1.5 0.5 0 0 0 - - - - 2 - 0 - 0 - 2 - 0 - 2 - - 2.0 - - + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + - - 0.5 - 1.0 - + + 0.1 0.1 0.1 + - - + - - 0.5 - 1.0 - + + 0.1 0.1 0.1 + - - 0 1 0 1 - 0 1 0 1 - 0 1 0 1 - - - 400.0 - + + + 1.047 + + 320 + 240 + L16 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera + + true - + 4.5 0 0.5 0.0 0.0 3.14 0.05 0.05 0.05 0 0 0 @@ -302,12 +276,13 @@ - + 1.047 320 240 + L8 0.1 @@ -317,11 +292,38 @@ 1 30 true - thermal_camera + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + true + + + 1 0 0 0 0 1.570796 + rescue_randy + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + + + + 2.25 .5 .1 0 0 1.570796 + phone + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black + + + + 2.25 -.5 .1 0 0 1.570796 + backpack + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Backpack + + diff --git a/examples/worlds/touch_plugin.sdf b/examples/worlds/touch_plugin.sdf index eed0d28593..7497a15984 100644 --- a/examples/worlds/touch_plugin.sdf +++ b/examples/worlds/touch_plugin.sdf @@ -55,6 +55,7 @@ The output of the plugin is via 0 0 1 + 100 100 @@ -136,4 +137,3 @@ The output of the plugin is via - diff --git a/examples/worlds/track_drive.sdf b/examples/worlds/track_drive.sdf index ed4c852a82..9680cd01e5 100644 --- a/examples/worlds/track_drive.sdf +++ b/examples/worlds/track_drive.sdf @@ -60,6 +60,7 @@ 0 0 1 + 100 100 @@ -2096,4 +2097,3 @@ - diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 110f10f0e1..e9812361c1 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -125,6 +125,7 @@ moment the box hits the ground, the second box should start falling. 0 0 1 + 100 100 @@ -532,4 +533,3 @@ moment the box hits the ground, the second box should start falling. - diff --git a/examples/worlds/trisphere_cycle_wheel_slip.sdf b/examples/worlds/trisphere_cycle_wheel_slip.sdf index bd7a23b219..7738361315 100644 --- a/examples/worlds/trisphere_cycle_wheel_slip.sdf +++ b/examples/worlds/trisphere_cycle_wheel_slip.sdf @@ -102,6 +102,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf index fcfabd8c12..2e104e5f08 100644 --- a/examples/worlds/velocity_control.sdf +++ b/examples/worlds/velocity_control.sdf @@ -119,6 +119,7 @@ 0 0 1 + 100 100 diff --git a/examples/worlds/video_record_dbl_pendulum.sdf b/examples/worlds/video_record_dbl_pendulum.sdf index 2894e0f7dd..ddbe423ff3 100644 --- a/examples/worlds/video_record_dbl_pendulum.sdf +++ b/examples/worlds/video_record_dbl_pendulum.sdf @@ -113,6 +113,7 @@ 0 0 1 + 100 100 @@ -334,4 +335,3 @@ - diff --git a/examples/worlds/wind.sdf b/examples/worlds/wind.sdf index f260d17f07..d5d58b73b7 100644 --- a/examples/worlds/wind.sdf +++ b/examples/worlds/wind.sdf @@ -109,6 +109,7 @@ Example: 0 0 1 + 100 100 diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index 2dd74c6c37..53d3b025ac 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -46,6 +47,7 @@ #include #include #include +#include #include #include @@ -432,6 +434,41 @@ namespace ignition sdf::Atmosphere convert(const msgs::Atmosphere &_in); + /// \brief Generic conversion from an SDF Physics to another type. + /// \param[in] _in SDF Physics. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Physics &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from an SDF physics to a physics + /// message. + /// \param[in] _in SDF physics. + /// \return Physics message. + template<> + msgs::Physics convert(const sdf::Physics &_in); + + /// \brief Generic conversion from a physics message to another type. + /// \param[in] _in Physics message. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Physics &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a physics message to a physics + /// SDF object. + /// \param[in] _in Physics message. + /// \return SDF physics. + template<> + sdf::Physics convert(const msgs::Physics &_in); + + /// \brief Generic conversion from an SDF Sensor to another type. /// \param[in] _in SDF Sensor. /// \return Conversion result. @@ -444,7 +481,7 @@ namespace ignition /// \brief Specialized conversion from an SDF sensor to a sensor /// message. - /// \param[in] _in SDF geometry. + /// \param[in] _in SDF sensor. /// \return Sensor message. template<> msgs::Sensor convert(const sdf::Sensor &_in); diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 90816df770..e2098d5fa1 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -215,6 +215,19 @@ namespace ignition public: template ComponentTypeT *Component(const ComponentKey &_key); + /// \brief Get a mutable component assigned to an entity based on a + /// component type. If the component doesn't exist, create it and + /// initialize with the given default value. + /// \param[in] _entity The entity. + /// \param[in] _default The value that should be used to construct + /// the component in case the component doesn't exist. + /// \return The component of the specified type assigned to the specified + /// entity. + public: template + ComponentTypeT *ComponentDefault(Entity _entity, + const typename ComponentTypeT::Type &_default = + typename ComponentTypeT::Type()); + /// \brief Get the data from a component. /// * If the component type doesn't hold any data, this won't compile. /// * If the entity doesn't have that component, it will return nullopt. @@ -488,6 +501,12 @@ namespace ignition /// \return True if there are any components with one-time changes. public: bool HasOneTimeComponentChanges() const; + /// \brief Get the components types that are marked as periodic changes. + /// \return All the components that at least one entity marked as + /// periodic changes. + public: std::unordered_set + ComponentTypesWithPeriodicChanges() const; + /// \brief Set the absolute state of the ECM from a serialized message. /// Entities / components that are in the new state but not in the old /// one will be created. diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 3ec9ebd54d..cfa49e98d3 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -134,7 +134,8 @@ namespace ignition /// \brief Get the top level model of an entity /// \param[in] _entity Input entity /// \param[in] _ecm Constant reference to ECM. - /// \return Entity of top level model + /// \return Entity of top level model. If _entity has no top level model, + /// kNullEntity is returned. ignition::gazebo::Entity IGNITION_GAZEBO_VISIBLE topLevelModel( const Entity &_entity, const EntityComponentManager &_ecm); diff --git a/include/ignition/gazebo/components/LaserRetro.hh b/include/ignition/gazebo/components/LaserRetro.hh new file mode 100644 index 0000000000..75d1145e92 --- /dev/null +++ b/include/ignition/gazebo/components/LaserRetro.hh @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2020 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_COMPONENTS_LASERRETRO_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LASERRETRO_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component used to indicate an lidar reflective value + using LaserRetro = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LaserRetro", + LaserRetro) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/LightCmd.hh b/include/ignition/gazebo/components/LightCmd.hh new file mode 100644 index 0000000000..fdba5858fc --- /dev/null +++ b/include/ignition/gazebo/components/LightCmd.hh @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2020 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_COMPONENTS_LIGHTCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LIGHTCMD_HH_ + +#include + +#include +#include +#include +#include +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains commanded light of an + /// entity in the world frame represented by msgs::Light. + using LightCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.LightCmd", + LightCmd) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/ParticleEmitter.hh b/include/ignition/gazebo/components/ParticleEmitter.hh new file mode 100644 index 0000000000..86fc36bd88 --- /dev/null +++ b/include/ignition/gazebo/components/ParticleEmitter.hh @@ -0,0 +1,52 @@ +/* + * 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_COMPONENTS_PARTICLEEMITTER_HH_ +#define IGNITION_GAZEBO_COMPONENTS_PARTICLEEMITTER_HH_ + +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component that contains a particle emitter. + using ParticleEmitter = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ParticleEmitter", + ParticleEmitter) + + /// \brief A component that contains a particle emitter command. + using ParticleEmitterCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ParticleEmitterCmd", + ParticleEmitterCmd) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/Physics.hh b/include/ignition/gazebo/components/Physics.hh new file mode 100644 index 0000000000..804059dedc --- /dev/null +++ b/include/ignition/gazebo/components/Physics.hh @@ -0,0 +1,56 @@ +/* + * 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_COMPONENTS_PHYSICS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_PHYSICS_HH_ + +#include + +#include + +#include +#include + +#include +#include "ignition/gazebo/components/Component.hh" +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using PhysicsSerializer = + serializers::ComponentToMsgSerializer; +} +namespace components +{ + /// \brief A component type that contains the physics properties of + /// the World entity. + using Physics = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Physics", + Physics) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/PhysicsCmd.hh b/include/ignition/gazebo/components/PhysicsCmd.hh new file mode 100644 index 0000000000..f72dc328b0 --- /dev/null +++ b/include/ignition/gazebo/components/PhysicsCmd.hh @@ -0,0 +1,46 @@ +/* + * 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_COMPONENTS_PHYSICSCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_PHYSICSCMD_HH_ + +#include + +#include +#include + +#include +#include "ignition/gazebo/components/Component.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains the physics properties of + /// the World entity. + using PhysicsCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.PhysicsCmd", + PhysicsCmd) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/Temperature.hh b/include/ignition/gazebo/components/Temperature.hh index 228e208518..cfd8df5327 100644 --- a/include/ignition/gazebo/components/Temperature.hh +++ b/include/ignition/gazebo/components/Temperature.hh @@ -35,6 +35,12 @@ namespace components using Temperature = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Temperature", Temperature) + + /// \brief A component that stores temperature linear resolution in Kelvin + using TemperatureLinearResolution = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.TemperatureLinearResolution", + TemperatureLinearResolution) } } } diff --git a/include/ignition/gazebo/components/TemperatureRange.hh b/include/ignition/gazebo/components/TemperatureRange.hh new file mode 100644 index 0000000000..99e4a26151 --- /dev/null +++ b/include/ignition/gazebo/components/TemperatureRange.hh @@ -0,0 +1,99 @@ +/* + * 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_COMPONENTS_TEMPERATURERANGE_HH_ +#define IGNITION_GAZEBO_COMPONENTS_TEMPERATURERANGE_HH_ + +#include +#include + +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief Data structure to hold a temperature range, in kelvin + struct TemperatureRangeInfo + { + /// \brief The minimum temperature (kelvin) + math::Temperature min; + /// \brief The maximum temperature (kelvin) + math::Temperature max; + + public: bool operator==(const TemperatureRangeInfo &_info) const + { + return (this->min == _info.min) && + (this->max == _info.max); + } + + public: bool operator!=(const TemperatureRangeInfo &_info) const + { + return !(*this == _info); + } + }; +} + +namespace serializers +{ + /// \brief Serializer for components::TemperatureRangeInfo object + class TemperatureRangeInfoSerializer + { + /// \brief Serialization for components::TemperatureRangeInfo + /// \param[out] _out Output stream + /// \param[in] _info Object for the stream + /// \return The stream + public: static std::ostream &Serialize(std::ostream &_out, + const components::TemperatureRangeInfo &_info) + { + _out << _info.min << " " << _info.max; + return _out; + } + + /// \brief Deserialization for components::TemperatureRangeInfo + /// \param[in] _in Input stream + /// \param[out] _info The object to populate + /// \return The stream + public: static std::istream &Deserialize(std::istream &_in, + components::TemperatureRangeInfo &_info) + { + _in >> _info.min >> _info.max; + return _in; + } + }; +} + +namespace components +{ + /// \brief A component that stores a temperature range in kelvin + using TemperatureRange = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.TemperatureRange", + TemperatureRange) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index cb6c2e368f..d08722ee80 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -128,6 +128,20 @@ ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key) this->ComponentImplementation(_key)); } +////////////////////////////////////////////////// +template +ComponentTypeT *EntityComponentManager::ComponentDefault(Entity _entity, + const typename ComponentTypeT::Type &_default) +{ + auto comp = this->Component(_entity); + if (!comp) + { + this->CreateComponent(_entity, ComponentTypeT(_default)); + comp = this->Component(_entity); + } + return comp; +} + ////////////////////////////////////////////////// template std::optional diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 0795d51bd8..8f21ff3625 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -66,6 +66,12 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Pointer to the scene public: rendering::ScenePtr Scene() const; + /// \brief Helper Update function for updating the scene + /// \param[in] _info Sim update info + /// \param[in] _ecm Mutable reference to the entity component manager + public: void UpdateECM(const UpdateInfo &_info, + EntityComponentManager &_ecm); + /// \brief Helper PostUpdate function for updating the scene public: void UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm); @@ -120,6 +126,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public : void SetRemoveSensorCb( std::function _removeSensorCb); + /// \brief View collisions of specified entity which are shown in orange + /// \param[in] _entity Entity to view collisions + public: void ViewCollisions(const Entity &_entity); + /// \brief Get the scene manager /// Returns reference to the scene manager. public: class SceneManager &SceneManager(); diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index fbd9e0e7ce..8ff571d1e0 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -34,6 +34,8 @@ #include #include +#include + #include #include @@ -132,6 +134,19 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); + /// \brief Create a collision visual + /// \param[in] _id Unique visual id + /// \param[in] _collision Collision sdf dom + /// \param[in] _parentId Parent id + /// \return Visual (collision) object created from the sdf dom + public: rendering::VisualPtr CreateCollision(Entity _id, + const sdf::Collision &_collision, Entity _parentId = 0); + + /// \brief Retrieve visual + /// \param[in] _id Unique visual (entity) id + /// \return Pointer to requested visual + public: rendering::VisualPtr VisualById(Entity _id); + /// \brief Create an actor /// \param[in] _id Unique actor id /// \param[in] _visual Actor sdf dom @@ -156,6 +171,21 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateLightVisual(Entity _id, const sdf::Light &_light, Entity _parentId); + /// \brief Create a particle emitter. + /// \param[in] _id Unique particle emitter id + /// \param[in] _emitter Particle emitter data + /// \param[in] _parentId Parent id + /// \return Default particle emitter object created + public: rendering::ParticleEmitterPtr CreateParticleEmitter( + Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId); + + /// \brief Update an existing particle emitter + /// \brief _id Emitter id to update + /// \brief _emitter Data to update the particle emitter + /// \return Particle emitter updated + public: rendering::ParticleEmitterPtr UpdateParticleEmitter(Entity _id, + const msgs::ParticleEmitter &_emitter); + /// \brief Ignition sensors is the one responsible for adding sensors /// to the scene. Here we just keep track of it and make sure it has /// the correct parent. diff --git a/src/Conversions.cc b/src/Conversions.cc index 166cce1a81..d7fb552857 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -839,6 +839,28 @@ void ignition::gazebo::set(msgs::WorldStatistics *_msg, _msg->set_paused(_in.paused); } +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +msgs::Physics ignition::gazebo::convert(const sdf::Physics &_in) +{ + msgs::Physics out; + out.set_max_step_size(_in.MaxStepSize()); + out.set_real_time_factor(_in.RealTimeFactor()); + return out; +} + +////////////////////////////////////////////////// +template<> +IGNITION_GAZEBO_VISIBLE +sdf::Physics ignition::gazebo::convert(const msgs::Physics &_in) +{ + sdf::Physics out; + out.SetRealTimeFactor(_in.real_time_factor()); + out.SetMaxStepSize(_in.max_step_size()); + return out; +} + ////////////////////////////////////////////////// void ignition::gazebo::set(msgs::SensorNoise *_msg, const sdf::Noise &_sdf) { diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 6ee1a3abab..fbb3dbd89d 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -29,6 +29,7 @@ #include #include #include +#include #include #include #include @@ -171,6 +172,28 @@ TEST(Conversions, Entity) EXPECT_EQ(msgs::Entity_Type_NONE, entityType2); } +///////////////////////////////////////////////// +TEST(Conversions, Physics) +{ + // Test conversion from msg to sdf + msgs::Physics msg; + msg.set_real_time_factor(1.23); + msg.set_max_step_size(0.12); + + auto physics = convert(msg); + EXPECT_DOUBLE_EQ(1.23, physics.RealTimeFactor()); + EXPECT_DOUBLE_EQ(0.12, physics.MaxStepSize()); + + // Test conversion from sdf to msg + sdf::Physics physSdf; + physSdf.SetMaxStepSize(0.34); + physSdf.SetRealTimeFactor(2.34); + + auto physMsg = convert(physSdf); + EXPECT_DOUBLE_EQ(2.34, physMsg.real_time_factor()); + EXPECT_DOUBLE_EQ(0.34, physMsg.max_step_size()); +} + ///////////////////////////////////////////////// TEST(Conversions, Pose) { diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 9a11236b81..0bb5662c95 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -50,17 +50,27 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \return True if created successfully. public: bool CreateComponentStorage(const ComponentTypeId _typeId); + /// \brief Allots the work for multiple threads prior to running + /// `AddEntityToMessage`. + public: void CalculateStateThreadLoad(); + /// \brief Create a message for the removed components /// \param[in] _entity Entity with the removed components - /// \param[out] _msg Entity message + /// \param[in, out] _msg Entity message + /// \param[in] _types _types Type IDs of components to be serialized. Leave + /// empty to get all removed components. public: void SetRemovedComponentsMsgs(Entity &_entity, - msgs::SerializedEntity *_msg); + msgs::SerializedEntity *_msg, + const std::unordered_set &_types = {}); /// \brief Create a message for the removed components /// \param[in] _entity Entity with the removed components - /// \param[out] _msg State message + /// \param[in, out] _msg State message + /// \param[in] _types _types Type IDs of components to be serialized. Leave + /// empty to get all removed components. public: void SetRemovedComponentsMsgs(Entity &_entity, - msgs::SerializedStateMap &_msg); + msgs::SerializedStateMap &_msg, + const std::unordered_set &_types = {}); /// \brief Map of component storage classes. The key is a component /// type id, and the value is a pointer to the component storage. @@ -86,10 +96,26 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Flag that indicates if all entities should be removed. public: bool removeAllEntities{false}; - /// \brief The set of components that each entity has + /// \brief True if the entityComponents map was changed. Primarily used + /// by the multithreading functionality in `State()` to allocate work to + /// each thread. + public: bool entityComponentsDirty{true}; + + /// \brief The set of components that each entity has. + /// NOTE: Any modification of this data structure must be followed + /// by setting `entityComponentsDirty` to true. public: std::unordered_map> entityComponents; + /// \brief A vector of iterators to evenly distributed spots in the + /// `entityComponents` map. Threads in the `State` function use this + /// vector for easy access of their pre-allocated work. This vector + /// is recalculated if `entityComponents` is changed (when + /// `entityComponentsDirty` == true). + public: std::vector>::iterator> + entityComponentIterators; + /// \brief A mutex to protect newly created entities. public: std::mutex entityCreatedMutex; @@ -249,6 +275,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests() this->dataPtr->entities = EntityGraph(); this->dataPtr->entityComponents.clear(); this->dataPtr->toRemoveEntities.clear(); + this->dataPtr->entityComponentsDirty = true; for (std::pair> &comp: this->dataPtr->components) @@ -284,6 +311,7 @@ void EntityComponentManager::ProcessRemoveEntityRequests() // Remove the entry in the entityComponent map this->dataPtr->entityComponents.erase(entity); + this->dataPtr->entityComponentsDirty = true; } // Remove the entity from views. @@ -322,6 +350,7 @@ bool EntityComponentManager::RemoveComponent( this->dataPtr->entityComponents[_entity].erase(_key.first); this->dataPtr->oneTimeChangedComponents.erase(_key); this->dataPtr->periodicChangedComponents.erase(_key); + this->dataPtr->entityComponentsDirty = true; this->UpdateViews(_entity); @@ -430,6 +459,18 @@ bool EntityComponentManager::HasOneTimeComponentChanges() const return !this->dataPtr->oneTimeChangedComponents.empty(); } +///////////////////////////////////////////////// +std::unordered_set + EntityComponentManager::ComponentTypesWithPeriodicChanges() const +{ + std::unordered_set periodicComponents; + for (const auto& compPair : this->dataPtr->periodicChangedComponents) + { + periodicComponents.insert(compPair.first); + } + return periodicComponents; +} + ///////////////////////////////////////////////// bool EntityComponentManager::HasEntity(const Entity _entity) const { @@ -497,6 +538,7 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( this->dataPtr->entityComponents[_entity].insert( {_componentTypeId, componentKey}); this->dataPtr->oneTimeChangedComponents.insert(componentKey); + this->dataPtr->entityComponentsDirty = true; if (componentIdPair.second) this->RebuildViews(); @@ -738,46 +780,63 @@ void EntityComponentManager::RebuildViews() ////////////////////////////////////////////////// void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, - msgs::SerializedEntity *_entityMsg) + msgs::SerializedEntity *_entityMsg, + const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); - uint64_t nEntityKeys = this->removedComponents.count(_entity); - if (nEntityKeys == 0) - return; - - auto it = this->removedComponents.find(_entity); - for (uint64_t i = 0; i < nEntityKeys; ++i) + auto entRemovedComps = this->removedComponents.equal_range(_entity); + for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) { - auto compMsg = _entityMsg->add_components(); - auto removedComponent = it->second; + if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + { + continue; + } + + auto compMsg = _entityMsg->add_components(); + // Empty data is needed for the component to be processed afterwards compMsg->set_component(" "); compMsg->set_type(removedComponent.first); compMsg->set_remove(true); - - it++; } } ////////////////////////////////////////////////// void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, - msgs::SerializedStateMap &_msg) + msgs::SerializedStateMap &_msg, + const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); uint64_t nEntityKeys = this->removedComponents.count(_entity); if (nEntityKeys == 0) return; - // Find the entity in the message + // The message need not necessarily contain the entity initially. For + // instance, when AddEntityToMessage() calls this function, the entity may + // have some removed components but none in entityComponents that changed, + // so the entity may not have been added to the message beforehand. auto entIter = _msg.mutable_entities()->find(_entity); + if (entIter == _msg.mutable_entities()->end()) + { + msgs::SerializedEntityMap ent; + ent.set_id(_entity); + entIter = + (_msg.mutable_entities())->insert({static_cast(_entity), ent}) + .first; + } - auto it = this->removedComponents.find(_entity); - for (uint64_t i = 0; i < nEntityKeys; ++i) + auto entRemovedComps = this->removedComponents.equal_range(_entity); + for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) { auto removedComponent = it->second; + if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + { + continue; + } + msgs::SerializedComponent compMsg; // Empty data is needed for the component to be processed afterwards @@ -787,8 +846,6 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, (*(entIter->second.mutable_components()))[ static_cast(removedComponent.first)] = compMsg; - - it++; } } @@ -842,7 +899,7 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, // Add a component to the message and set it to be removed if the component // exists in the removedComponents map. - this->dataPtr->SetRemovedComponentsMsgs(_entity, entityMsg); + this->dataPtr->SetRemovedComponentsMsgs(_entity, entityMsg, _types); } ////////////////////////////////////////////////// @@ -944,7 +1001,7 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, // Add a component to the message and set it to be removed if the component // exists in the removedComponents map. - this->dataPtr->SetRemovedComponentsMsgs(_entity, _msg); + this->dataPtr->SetRemovedComponentsMsgs(_entity, _msg, _types); } ////////////////////////////////////////////////// @@ -988,6 +1045,51 @@ void EntityComponentManager::ChangedState( // TODO(anyone) New / removed / changed components } +////////////////////////////////////////////////// +void EntityComponentManagerPrivate::CalculateStateThreadLoad() +{ + // If the entity component vector is dirty, we need to recalculate the + // threads and each threads work load + if (!this->entityComponentsDirty) + return; + + this->entityComponentsDirty = false; + this->entityComponentIterators.clear(); + auto startIt = this->entityComponents.begin(); + int numComponents = this->entityComponents.size(); + + // Set the number of threads to spawn to the min of the calculated thread + // count or max threads that the hardware supports + int maxThreads = std::thread::hardware_concurrency(); + uint64_t numThreads = std::min(numComponents, maxThreads); + + int componentsPerThread = std::ceil(static_cast(numComponents) / + numThreads); + + igndbg << "Updated state thread iterators: " << numThreads + << " threads processing around " << componentsPerThread + << " components each." << std::endl; + + // Push back the starting iterator + this->entityComponentIterators.push_back(startIt); + for (uint64_t i = 0; i < numThreads; ++i) + { + // If we have added all of the components to the iterator vector, we are + // done so push back the end iterator + numComponents -= componentsPerThread; + if (numComponents <= 0) + { + this->entityComponentIterators.push_back( + this->entityComponents.end()); + break; + } + + // Get the iterator to the next starting group of components + auto nextIt = std::next(startIt, componentsPerThread); + this->entityComponentIterators.push_back(nextIt); + startIt = nextIt; + } +} ////////////////////////////////////////////////// ignition::msgs::SerializedState EntityComponentManager::State( @@ -1016,11 +1118,46 @@ void EntityComponentManager::State( const std::unordered_set &_types, bool _full) const { - for (const auto &it : this->dataPtr->entityComponents) + std::mutex stateMapMutex; + std::vector workers; + + this->dataPtr->CalculateStateThreadLoad(); + + auto functor = [&](auto itStart, auto itEnd) + { + msgs::SerializedStateMap threadMap; + while (itStart != itEnd) + { + auto entity = (*itStart).first; + if (_entities.empty() || _entities.find(entity) != _entities.end()) + { + this->AddEntityToMessage(threadMap, entity, _types, _full); + } + itStart++; + } + std::lock_guard lock(stateMapMutex); + + for (const auto &entity : threadMap.entities()) + { + (*_state.mutable_entities())[static_cast(entity.first)] = + entity.second; + } + }; + + // Spawn workers + uint64_t numThreads = this->dataPtr->entityComponentIterators.size() - 1; + for (uint64_t i = 0; i < numThreads; i++) { - if (_entities.empty() || _entities.find(it.first) != _entities.end()) - this->AddEntityToMessage(_state, it.first, _types, _full); + workers.push_back(std::thread(functor, + this->dataPtr->entityComponentIterators[i], + this->dataPtr->entityComponentIterators[i+1])); } + + // Wait for each thread to finish processing its components + std::for_each(workers.begin(), workers.end(), [](std::thread &_t) + { + _t.join(); + }); } ////////////////////////////////////////////////// diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 5e03c68d79..cf42727bdd 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -437,6 +437,31 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_FALSE(manager.EntityHasComponentType(entity, DoubleComponent::typeId)); EXPECT_FALSE(manager.EntityHasComponentType(entity2, IntComponent::typeId)); + // Query non-existing component, the default value is default-constructed + BoolComponent *boolComp = manager.ComponentDefault(entity); + ASSERT_NE(nullptr, boolComp); + EXPECT_TRUE(manager.HasComponentType(BoolComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(entity, BoolComponent::typeId)); + EXPECT_EQ(false, boolComp->Data()); + + // Query non-existing component, the default value is used + DoubleComponent *doubleComp = + manager.ComponentDefault(entity, 1.0); + ASSERT_NE(nullptr, doubleComp); + EXPECT_TRUE(manager.HasComponentType(DoubleComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(entity, DoubleComponent::typeId)); + EXPECT_FALSE( + manager.EntityHasComponentType(entity2, DoubleComponent::typeId)); + EXPECT_FLOAT_EQ(1.0, doubleComp->Data()); + + // Query existing component, the default value is not used + IntComponent *intComp = manager.ComponentDefault(entity, 124); + ASSERT_NE(nullptr, intComp); + EXPECT_TRUE(manager.HasComponentType(IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); + EXPECT_EQ(123, intComp->Data()); + // Remove all entities manager.RequestRemoveEntities(); EXPECT_EQ(3u, manager.EntityCount()); @@ -2086,6 +2111,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) auto c2 = manager.CreateComponent(e2, IntComponent(456)); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::OneTimeChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::OneTimeChange, @@ -2097,6 +2123,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // updated manager.RunSetAllComponentsUnchanged(); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::NoChange, @@ -2104,9 +2131,31 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // Mark as changed manager.SetChanged(e1, c1.first, ComponentState::PeriodicChange); + + // check that only e1 c1 is serialized into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + { + ASSERT_EQ(1, stateMsg.entities_size()); + + auto iter = stateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + EXPECT_EQ(e1, e1Msg.id()); + ASSERT_EQ(1, e1Msg.components_size()); + + auto compIter = e1Msg.components().begin(); + const auto &e1c1Msg = compIter->second; + EXPECT_EQ(IntComponent::typeId, e1c1Msg.type()); + EXPECT_EQ(123, std::stoi(e1c1Msg.component())); + } + manager.SetChanged(e2, c2.first, ComponentState::OneTimeChange); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + // Expect a single component type to be marked as PeriodicChange + ASSERT_EQ(1u, manager.ComponentTypesWithPeriodicChanges().size()); + EXPECT_EQ(IntComponent().TypeId(), + *manager.ComponentTypesWithPeriodicChanges().begin()); EXPECT_EQ(ComponentState::PeriodicChange, manager.ComponentState(e1, c1.first)); EXPECT_EQ(ComponentState::OneTimeChange, @@ -2116,6 +2165,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_TRUE(manager.RemoveComponent(e1, c1.first)); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); + EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, c1.first)); @@ -2263,6 +2313,86 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMsgAfterRemoveComponent) } } +////////////////////////////////////////////////// +// Verify SerializedStateMap message with no changed components, +// but some removed components +TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgCompsRemovedOnly) +{ + // Create entity + Entity e1 = manager.CreateEntity(); + auto e1c0 = + manager.CreateComponent(e1, IntComponent(123)); + manager.CreateComponent(e1, DoubleComponent(0.0)); + auto e1c2 = + manager.CreateComponent(e1, StringComponent("int")); + + manager.RunSetAllComponentsUnchanged(); + manager.RemoveComponent(e1, e1c0); + manager.RemoveComponent(e1, e1c2); + // Serialize into a message + msgs::SerializedStateMap stateMsg; + manager.State(stateMsg); + + // Check message + { + auto iter = stateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + auto compIter = e1Msg.components().begin(); + + // Check number of components + ASSERT_EQ(e1Msg.components().size(), 2u); + + // First component + const auto &c0 = compIter->second; + compIter++; + ASSERT_EQ(c0.remove(), true); + + // Second component + const auto &c2 = compIter->second; + ASSERT_EQ(c2.remove(), true); + } +} + +////////////////////////////////////////////////// +// Verify that removed components are correctly filtered when creating a +// SerializedStateMap message +TEST_P(EntityComponentManagerFixture, SetRemovedComponentsMsgTypesFilter) +{ + // Create entity + Entity e1 = manager.CreateEntity(); + auto e1c0 = + manager.CreateComponent(e1, IntComponent(123)); + auto e1c1 = + manager.CreateComponent(e1, DoubleComponent(0.0)); + auto e1c2 = + manager.CreateComponent(e1, StringComponent("foo")); + + manager.RunSetAllComponentsUnchanged(); + manager.RemoveComponent(e1, e1c0); + manager.RemoveComponent(e1, e1c2); + + // Serialize into a message, providing a list of types to be included + msgs::SerializedStateMap stateMsg; + std::unordered_set entitySet{e1}; + std::unordered_set types{e1c0.first, e1c1.first}; + manager.State(stateMsg, entitySet, types, false); + + // Check message + { + auto iter = stateMsg.entities().find(e1); + const auto &e1Msg = iter->second; + auto compIter = e1Msg.components().begin(); + + // Check number of components + ASSERT_EQ(e1Msg.components().size(), 1u); + + // Only component in message should be e1c2 + const auto &c0 = compIter->second; + EXPECT_EQ(c0.remove(), true); + EXPECT_EQ(c0.type(), e1c0.first); + } +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) { diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 1b54f01848..74e2f71020 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -46,6 +46,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerLevels.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" @@ -102,6 +103,14 @@ void LevelManager::ReadLevelPerformerInfo() this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::Gravity(this->runner->sdfWorld->Gravity())); + auto physics = this->runner->sdfWorld->PhysicsByIndex(0); + if (!physics) + { + physics = this->runner->sdfWorld->PhysicsDefault(); + } + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::Physics(*physics)); + this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::MagneticField(this->runner->sdfWorld->MagneticField())); diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 380b28911d..42f06fe948 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -41,6 +41,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Lidar.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightType.hh" @@ -55,6 +56,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" @@ -203,6 +205,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) this->dataPtr->ecm->CreateComponent(worldEntity, components::Gravity(_world->Gravity())); + // Physics + // \todo(anyone) Support picking a specific physics profile + auto physics = _world->PhysicsByIndex(0); + if (!physics) + { + physics = _world->PhysicsDefault(); + } + this->dataPtr->ecm->CreateComponent(worldEntity, + components::Physics(*physics)); + // MagneticField this->dataPtr->ecm->CreateComponent(worldEntity, components::MagneticField(_world->MagneticField())); @@ -514,6 +526,12 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Visual *_visual) this->dataPtr->ecm->CreateComponent(visualEntity, components::VisibilityFlags(_visual->VisibilityFlags())); + if (_visual->HasLaserRetro()) + { + this->dataPtr->ecm->CreateComponent(visualEntity, + components::LaserRetro(_visual->LaserRetro())); + } + if (_visual->Geom()) { this->dataPtr->ecm->CreateComponent(visualEntity, diff --git a/src/SdfEntityCreator_TEST.cc b/src/SdfEntityCreator_TEST.cc index b46eb95322..0cad9949c4 100644 --- a/src/SdfEntityCreator_TEST.cc +++ b/src/SdfEntityCreator_TEST.cc @@ -35,6 +35,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" #include "ignition/gazebo/components/JointType.hh" +#include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Material.hh" @@ -42,6 +43,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visibility.hh" @@ -102,6 +104,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_TRUE(this->ecm.HasComponentType(components::Geometry::typeId)); EXPECT_TRUE(this->ecm.HasComponentType(components::Material::typeId)); EXPECT_TRUE(this->ecm.HasComponentType(components::Inertial::typeId)); + EXPECT_TRUE(this->ecm.HasComponentType(components::LaserRetro::typeId)); // Check entities // 1 x world + 3 x model + 3 x link + 3 x collision + 3 x visual + 1 x light @@ -111,15 +114,20 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) unsigned int worldCount{0}; Entity worldEntity = kNullEntity; this->ecm.Each( + components::Name, + components::Physics>( [&](const Entity &_entity, const components::World *_world, - const components::Name *_name)->bool + const components::Name *_name, + const components::Physics *_physics)->bool { EXPECT_NE(nullptr, _world); EXPECT_NE(nullptr, _name); + EXPECT_NE(nullptr, _physics); EXPECT_EQ("default", _name->Data()); + EXPECT_DOUBLE_EQ(0.001, _physics->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(1.0, _physics->Data().RealTimeFactor()); worldCount++; @@ -357,6 +365,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) unsigned int visualCount{0}; this->ecm.Eachecm.ParentEntity(_entity)); EXPECT_DOUBLE_EQ(0.0, _transparency->Data()); + EXPECT_DOUBLE_EQ(1150.0, _laserRetro->Data()); EXPECT_TRUE(_castShadows->Data()); EXPECT_EQ(sdf::GeometryType::BOX, _geometry->Data().Type()); @@ -423,6 +434,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_EQ(cylLinkEntity, this->ecm.ParentEntity(_entity)); EXPECT_DOUBLE_EQ(0.0, _transparency->Data()); + EXPECT_DOUBLE_EQ(1654.0, _laserRetro->Data()); EXPECT_TRUE(_castShadows->Data()); EXPECT_EQ(sdf::GeometryType::CYLINDER, _geometry->Data().Type()); @@ -448,6 +460,7 @@ TEST_F(SdfEntityCreatorTest, CreateEntities) EXPECT_EQ(sphLinkEntity, this->ecm.ParentEntity(_entity)); EXPECT_DOUBLE_EQ(0.5, _transparency->Data()); + EXPECT_DOUBLE_EQ(50.0, _laserRetro->Data()); EXPECT_FALSE(_castShadows->Data()); EXPECT_EQ(sdf::GeometryType::SPHERE, _geometry->Data().Type()); diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index beee1e2ba4..b7dd413cb6 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -572,9 +572,9 @@ ServerConfig::LogPlaybackPlugin() const if (!this->LogPlaybackPath().empty()) { sdf::ElementPtr pathElem = std::make_shared(); - pathElem->SetName("path"); + pathElem->SetName("playback_path"); playbackElem->AddElementDescription(pathElem); - pathElem = playbackElem->GetElement("path"); + pathElem = playbackElem->GetElement("playback_path"); pathElem->AddValue("string", "", false, ""); pathElem->Set(this->LogPlaybackPath()); } diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index d082232740..f49b6123e8 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -227,15 +227,15 @@ void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) bool sdfUseLogRecord = (recordPluginElem != nullptr); bool hasRecordResources {false}; - bool hasCompress {false}; bool hasRecordTopics {false}; bool sdfRecordResources; - bool sdfCompress; std::vector sdfRecordTopics; if (sdfUseLogRecord) { + bool hasCompress {false}; + bool sdfCompress; std::tie(sdfRecordResources, hasRecordResources) = recordPluginElem->Get("record_resources", false); std::tie(sdfCompress, hasCompress) = @@ -349,7 +349,7 @@ void ServerPrivate::SetupTransport() if (this->node.Advertise(getPathService, &ServerPrivate::ResourcePathsService, this)) { - ignmsg << "Resource path get service on [" << addPathService << "]." + ignmsg << "Resource path get service on [" << getPathService << "]." << std::endl; } else diff --git a/src/Server_TEST.cc b/src/Server_TEST.cc index ee130a6ef9..e2ee8eed0f 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -18,7 +18,6 @@ #include #include #include -#include #include #include #include @@ -39,23 +38,12 @@ #include "plugins/MockSystem.hh" #include "../test/helpers/Relay.hh" +#include "../test/helpers/EnvTestFixture.hh" using namespace ignition; using namespace ignition::gazebo; using namespace std::chrono_literals; -class ServerFixture : public ::testing::TestWithParam -{ - protected: void SetUp() override - { - // Augment the system plugin path. In SetUp to avoid test order issues. - ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", - (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); - - ignition::common::Console::SetVerbosity(4); - } -}; - ///////////////////////////////////////////////// TEST_P(ServerFixture, DefaultServerConfig) { @@ -624,79 +612,6 @@ TEST_P(ServerFixture, SigInt) EXPECT_FALSE(*server.Running(0)); } -///////////////////////////////////////////////// -TEST_P(ServerFixture, TwoServersNonBlocking) -{ - ignition::gazebo::ServerConfig serverConfig; - serverConfig.SetSdfString(TestWorldSansPhysics::World()); - - gazebo::Server server1(serverConfig); - gazebo::Server server2(serverConfig); - EXPECT_FALSE(server1.Running()); - EXPECT_FALSE(*server1.Running(0)); - EXPECT_FALSE(server2.Running()); - EXPECT_FALSE(*server2.Running(0)); - EXPECT_EQ(0u, *server1.IterationCount()); - EXPECT_EQ(0u, *server2.IterationCount()); - - // Make the servers run fast. - server1.SetUpdatePeriod(1ns); - server2.SetUpdatePeriod(1ns); - - // Start non-blocking - const size_t iters1 = 9999; - EXPECT_TRUE(server1.Run(false, iters1, false)); - - // Expect that we can't start another instance. - EXPECT_FALSE(server1.Run(true, 10, false)); - - // It's okay to start another server - EXPECT_TRUE(server2.Run(false, 500, false)); - - while (*server1.IterationCount() < iters1 || *server2.IterationCount() < 500) - IGN_SLEEP_MS(100); - - EXPECT_EQ(iters1, *server1.IterationCount()); - EXPECT_EQ(500u, *server2.IterationCount()); - EXPECT_FALSE(server1.Running()); - EXPECT_FALSE(*server1.Running(0)); - EXPECT_FALSE(server2.Running()); - EXPECT_FALSE(*server2.Running(0)); -} - -///////////////////////////////////////////////// -TEST_P(ServerFixture, TwoServersMixedBlocking) -{ - ignition::gazebo::ServerConfig serverConfig; - serverConfig.SetSdfString(TestWorldSansPhysics::World()); - - gazebo::Server server1(serverConfig); - gazebo::Server server2(serverConfig); - EXPECT_FALSE(server1.Running()); - EXPECT_FALSE(*server1.Running(0)); - EXPECT_FALSE(server2.Running()); - EXPECT_FALSE(*server2.Running(0)); - EXPECT_EQ(0u, *server1.IterationCount()); - EXPECT_EQ(0u, *server2.IterationCount()); - - // Make the servers run fast. - server1.SetUpdatePeriod(1ns); - server2.SetUpdatePeriod(1ns); - - server1.Run(false, 10, false); - server2.Run(true, 1000, false); - - while (*server1.IterationCount() < 10) - IGN_SLEEP_MS(100); - - EXPECT_EQ(10u, *server1.IterationCount()); - EXPECT_EQ(1000u, *server2.IterationCount()); - EXPECT_FALSE(server1.Running()); - EXPECT_FALSE(*server1.Running(0)); - EXPECT_FALSE(server2.Running()); - EXPECT_FALSE(*server2.Running(0)); -} - ///////////////////////////////////////////////// TEST_P(ServerFixture, AddSystemWhileRunning) { @@ -902,32 +817,6 @@ TEST_P(ServerFixture, GetResourcePaths) EXPECT_EQ("/home/user/another_path", res.data(1)); } -///////////////////////////////////////////////// -TEST_P(ServerFixture, CachedFuelWorld) -{ - auto cachedWorldPath = - common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds"); - ignition::common::setenv("IGN_FUEL_CACHE_PATH", cachedWorldPath.c_str()); - - ServerConfig serverConfig; - auto fuelWorldURL = - "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/worlds/Test%20world"; - EXPECT_TRUE(serverConfig.SetSdfFile(fuelWorldURL)); - - EXPECT_EQ(fuelWorldURL, serverConfig.SdfFile()); - EXPECT_TRUE(serverConfig.SdfString().empty()); - - // Check that world was loaded - auto server = Server(serverConfig); - EXPECT_NE(std::nullopt, server.Running(0)); - EXPECT_FALSE(*server.Running(0)); - - server.Run(true /*blocking*/, 1, false/*paused*/); - - EXPECT_NE(std::nullopt, server.Running(0)); - EXPECT_FALSE(*server.Running(0)); -} - ///////////////////////////////////////////////// TEST_P(ServerFixture, AddResourcePaths) { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index f606e76f8b..1e52182873 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -27,6 +27,8 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/components/Physics.hh" +#include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/Events.hh" #include "ignition/gazebo/SdfEntityCreator.hh" #include "ignition/gazebo/Util.hh" @@ -60,8 +62,8 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Keep system loader so plugins can be loaded at runtime this->systemLoader = _systemLoader; - // Get the first physics profile - // \todo(louise) Support picking a specific profile + // Get the physics profile + // TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager auto physics = _world->PhysicsByIndex(0); if (!physics) { @@ -76,7 +78,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, dur); // Desired real time factor - double desiredRtf = _world->PhysicsDefault()->RealTimeFactor(); + this->desiredRtf = physics->RealTimeFactor(); // The instantaneous real time factor is given as: // @@ -102,7 +104,7 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // // period = step_size / RTF this->updatePeriod = std::chrono::nanoseconds( - static_cast(this->stepSize.count() / desiredRtf)); + static_cast(this->stepSize.count() / this->desiredRtf)); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -330,6 +332,47 @@ void SimulationRunner::UpdateCurrentInfo() } } +///////////////////////////////////////////////// +void SimulationRunner::UpdatePhysicsParams() +{ + auto worldEntity = + this->entityCompMgr.EntityByComponents(components::World()); + const auto physicsCmdComp = + this->entityCompMgr.Component(worldEntity); + if (!physicsCmdComp) + { + return; + } + auto physicsComp = + this->entityCompMgr.Component(worldEntity); + + const auto& physicsParams = physicsCmdComp->Data(); + const auto newStepSize = + std::chrono::duration(physicsParams.max_step_size()); + const double newRTF = physicsParams.real_time_factor(); + + const double eps = 0.00001; + 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); + } + this->entityCompMgr.RemoveComponent(worldEntity); +} + ///////////////////////////////////////////////// void SimulationRunner::PublishStats() { @@ -647,6 +690,10 @@ bool SimulationRunner::Run(const uint64_t _iterations) processedIterations < _iterations)) { IGN_PROFILE("SimulationRunner::Run - Iteration"); + + // Update the step size and desired rtf + this->UpdatePhysicsParams(); + // Compute the time to sleep in order to match, as closely as possible, // the update period. sleepTime = 0ns; @@ -881,18 +928,18 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) { std::list plugins; - if(_config.UseLogRecord() && !_config.LogPlaybackPath().empty()) + if (_config.UseLogRecord() && !_config.LogPlaybackPath().empty()) { ignwarn << "Both recording and playback are specified, defaulting to playback\n"; } - if(!_config.LogPlaybackPath().empty()) + if (!_config.LogPlaybackPath().empty()) { auto playbackPlugin = _config.LogPlaybackPlugin(); plugins.push_back(playbackPlugin); } - else if(_config.UseLogRecord()) + else if (_config.UseLogRecord()) { auto recordPlugin = _config.LogRecordPlugin(); plugins.push_back(recordPlugin); diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index d8b39ebe69..2c5dda2763 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -384,6 +384,10 @@ namespace ignition /// \brief Set the next step to be blocking and paused. public: void SetNextStepAsBlockingPaused(const bool value); + /// \brief Updates the physics parameters of the simulation based on the + /// Physics component of the world, if any. + public: void UpdatePhysicsParams(); + /// \brief This is used to indicate that a stop event has been received. private: std::atomic stopReceived{false}; @@ -474,6 +478,9 @@ namespace ignition /// \brief Step size private: ignition::math::clock::duration stepSize{10ms}; + /// \brief Desired real time factor + private: double desiredRtf{1.0}; + /// \brief Connection to the pause event. private: ignition::common::ConnectionPtr pauseConn; diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index 4045f1c83a..701d55f85f 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -177,6 +177,10 @@ TEST_P(SimulationRunnerTest, CreateEntities) EXPECT_EQ(1u, worldCount); EXPECT_NE(kNullEntity, worldEntity); + // Test step size, real time factor is not testable since it has no public API + auto stepSize = std::chrono::duration(runner.StepSize()).count(); + EXPECT_DOUBLE_EQ(0.001, stepSize); + // Check models unsigned int modelCount{0}; Entity boxModelEntity = kNullEntity; diff --git a/src/Util.cc b/src/Util.cc index f78ed00b25..5f2e4740f9 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -416,22 +416,23 @@ ignition::gazebo::Entity topLevelModel(const Entity &_entity, { auto entity = _entity; - // check if parent is a model - auto parentComp = _ecm.Component(entity); - while (parentComp) - { - // check if parent is a model - auto parentEntity = parentComp->Data(); - auto modelComp = _ecm.Component( - parentEntity); - if (!modelComp) + // search up the entity tree and find the model with no parent models + // (there is the possibility of nested models) + Entity modelEntity = kNullEntity; + while (entity) + { + if (_ecm.Component(entity)) + modelEntity = entity; + + // stop searching if we are at the root of the tree + auto parentComp = _ecm.Component(entity); + if (!parentComp) break; - // set current model entity - entity = parentEntity; - parentComp = _ecm.Component(entity); + entity = parentComp->Data(); } - return entity; + + return modelEntity; } ////////////////////////////////////////////////// diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index d699b3dc3b..f9edf2543b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -428,6 +428,7 @@ TEST_F(UtilTest, TopLevelModel) // - linkA // - modelB // - linkB + // - visualB // - modelC // World @@ -459,20 +460,31 @@ TEST_F(UtilTest, TopLevelModel) ecm.CreateComponent(linkBEntity, components::Name("linkB_name")); ecm.CreateComponent(linkBEntity, components::ParentEntity(modelBEntity)); + // Visual B - child of Link B + auto visualBEntity = ecm.CreateEntity(); + ecm.CreateComponent(visualBEntity, components::Visual()); + ecm.CreateComponent(visualBEntity, components::Name("visualB_name")); + ecm.CreateComponent(visualBEntity, 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)); - // model A, link A, model B and link B should have model A as top level entity + // model A, link A, model B, link B and visual B should have + // model A as the top level model EXPECT_EQ(modelAEntity, topLevelModel(modelAEntity, ecm)); EXPECT_EQ(modelAEntity, topLevelModel(linkAEntity, ecm)); EXPECT_EQ(modelAEntity, topLevelModel(modelBEntity, ecm)); EXPECT_EQ(modelAEntity, topLevelModel(linkBEntity, ecm)); + EXPECT_EQ(modelAEntity, topLevelModel(visualBEntity, ecm)); - // model C should have itself as the top level entity + // model C should have itself as the top level model EXPECT_EQ(modelCEntity, topLevelModel(modelCEntity, ecm)); + + // the world should have no top level model + EXPECT_EQ(kNullEntity, topLevelModel(worldEntity, ecm)); } ///////////////////////////////////////////////// diff --git a/src/gui/AboutDialogHandler.cc b/src/gui/AboutDialogHandler.cc new file mode 100644 index 0000000000..545f6fbb66 --- /dev/null +++ b/src/gui/AboutDialogHandler.cc @@ -0,0 +1,67 @@ +/* + * 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 "AboutDialogHandler.hh" + +#include +#include +#include + +using namespace ignition; +using namespace gazebo; +using namespace gazebo::gui; + +///////////////////////////////////////////////// +AboutDialogHandler::AboutDialogHandler() +{ + aboutText += std::string(IGNITION_GAZEBO_VERSION_HEADER); + aboutText += "" + "" + "" + "" + "" + "" + "" + "" + "" + "
Documentation:" + "" + "" + "https://ignitionrobotics.org/libs/gazebo" + "" + "
" + "Tutorials:" + "" + "" + "https://ignitionrobotics.org/docs/" + "" + "
"; +} + +///////////////////////////////////////////////// +QString AboutDialogHandler::getVersionInformation() +{ + return QString::fromStdString(this->aboutText); +} + +///////////////////////////////////////////////// +void AboutDialogHandler::openURL(QString _url) +{ + QDesktopServices::openUrl(QUrl(_url)); +} diff --git a/src/gui/AboutDialogHandler.hh b/src/gui/AboutDialogHandler.hh new file mode 100644 index 0000000000..14f1bbd20c --- /dev/null +++ b/src/gui/AboutDialogHandler.hh @@ -0,0 +1,58 @@ +/* + * 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_GUI_ABOUTDIALOGHANDLER_HH_ +#define IGNITION_GAZEBO_GUI_ABOUTDIALOGHANDLER_HH_ + +#include +#include +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Export.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace gui +{ +/// \brief Class for handling about dialog +class AboutDialogHandler : public QObject +{ + Q_OBJECT + + /// \brief Constructor + public: AboutDialogHandler(); + + /// \brief Get version information + /// \return Version information in rich text format + Q_INVOKABLE QString getVersionInformation(); + + /// \brief Function called from QML when user clicks on a link + /// \param[in] _url Url to web page. + Q_INVOKABLE void openURL(QString _url); + + /// \brief Version information and links to online resources + private: std::string aboutText; +}; +} +} +} +} +#endif diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index 1d96ff759a..efcc595f41 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -1,4 +1,5 @@ set (gui_sources + AboutDialogHandler.cc Gui.cc GuiFileHandler.cc GuiRunner.cc diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index 46ad5d5814..58056f40cf 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -27,6 +27,7 @@ #include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/gui/Gui.hh" +#include "AboutDialogHandler.hh" #include "GuiFileHandler.hh" #include "PathManager.hh" @@ -64,6 +65,9 @@ std::unique_ptr createGui( auto app = std::make_unique(_argc, _argv); app->AddPluginPath(IGN_GAZEBO_GUI_PLUGIN_INSTALL_DIR); + auto aboutDialogHandler = new ignition::gazebo::gui::AboutDialogHandler(); + aboutDialogHandler->setParent(app->Engine()); + auto guiFileHandler = new ignition::gazebo::gui::GuiFileHandler(); guiFileHandler->setParent(app->Engine()); @@ -102,6 +106,7 @@ std::unique_ptr createGui( // Let QML files use C++ functions and properties auto context = new QQmlContext(app->Engine()->rootContext()); + context->setContextProperty("AboutDialogHandler", aboutDialogHandler); context->setContextProperty("GuiFileHandler", guiFileHandler); // Instantiate GazeboDrawer.qml file into a component diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index 96d8aa35a2..aefa833f43 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -95,7 +95,17 @@ void GuiRunner::RequestState() } reqSrv = reqSrvValid; - this->dataPtr->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, this); + auto advertised = this->dataPtr->node.AdvertisedServices(); + if (std::find(advertised.begin(), advertised.end(), reqSrv) == + advertised.end()) + { + if (!this->dataPtr->node.Advertise(reqSrv, &GuiRunner::OnStateAsyncService, + this)) + { + ignerr << "Failed to advertise [" << reqSrv << "]" << std::endl; + } + } + ignition::msgs::StringMsg req; req.set_data(reqSrv); @@ -114,7 +124,7 @@ void GuiRunner::OnPluginAdded(const QString &_objectName) return; } - plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); + this->RequestState(); } ///////////////////////////////////////////////// diff --git a/src/gui/PathManager.cc b/src/gui/PathManager.cc index 575e8de6ec..a1f920519a 100644 --- a/src/gui/PathManager.cc +++ b/src/gui/PathManager.cc @@ -44,21 +44,30 @@ void onAddResourcePaths(const msgs::StringMsg_V &_msg) addResourcePaths(paths); } +////////////////////////////////////////////////// +void onAddResourcePaths(const msgs::StringMsg_V &_res, const bool _result) +{ + if (!_result) + { + ignerr << "Failed to get resource paths through service" << std::endl; + return; + } + igndbg << "Received resource paths." << std::endl; + + onAddResourcePaths(_res); +} + ///////////////////////////////////////////////// PathManager::PathManager() { - // Get resource paths + // Trigger an initial request to get all paths from server std::string service{"/gazebo/resource_paths/get"}; - msgs::StringMsg_V res; - bool result{false}; - auto executed = this->node.Request(service, 5000, res, result); - if (!executed) - ignerr << "Service call timed out for [" << service << "]" << std::endl; - else if (!result) - ignerr << "Service call failed for [" << service << "]" << std::endl; - - onAddResourcePaths(res); + igndbg << "Requesting resource paths through [" << service << "]" + << std::endl; + this->node.Request(service, onAddResourcePaths); + // Get path updates through this topic this->node.Subscribe("/gazebo/resource_paths", onAddResourcePaths); } + diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index a19fbb39ee..b203ea3700 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -35,6 +35,7 @@ #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/Level.hh" #include "ignition/gazebo/components/Light.hh" +#include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/LightType.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" @@ -47,6 +48,7 @@ #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerAffinity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/SelfCollide.hh" @@ -77,6 +79,9 @@ namespace ignition::gazebo /// \brief Name of the world public: std::string worldName; + /// \brief Entity name + public: std::string entityName; + /// \brief Entity type, such as 'world' or 'model'. public: QString type; @@ -113,6 +118,50 @@ void ignition::gazebo::setData(QStandardItem *_item, const math::Pose3d &_data) }), ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, const msgs::Light &_data) +{ + int lightType = -1; + if (_data.type() == msgs::Light::POINT) + { + lightType = 0; + } + else if (_data.type() == msgs::Light::SPOT) + { + lightType = 1; + } + else if (_data.type() == msgs::Light::DIRECTIONAL) + { + lightType = 2; + } + + _item->setData(QString("Light"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(_data.specular().r()), + QVariant(_data.specular().g()), + QVariant(_data.specular().b()), + QVariant(_data.specular().a()), + QVariant(_data.diffuse().r()), + QVariant(_data.diffuse().g()), + QVariant(_data.diffuse().b()), + QVariant(_data.diffuse().a()), + QVariant(_data.range()), + QVariant(_data.attenuation_linear()), + QVariant(_data.attenuation_constant()), + QVariant(_data.attenuation_quadratic()), + QVariant(_data.cast_shadows()), + QVariant(_data.direction().x()), + QVariant(_data.direction().y()), + QVariant(_data.direction().z()), + QVariant(_data.spot_inner_angle()), + QVariant(_data.spot_outer_angle()), + QVariant(_data.spot_falloff()), + QVariant(lightType) + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// template<> void ignition::gazebo::setData(QStandardItem *_item, @@ -175,6 +224,18 @@ void ignition::gazebo::setData(QStandardItem *_item, const double &_data) _item->setData(_data, ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) +{ + _item->setData(QString("Physics"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(_data.MaxStepSize()), + QVariant(_data.RealTimeFactor()) + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { @@ -371,12 +432,6 @@ void ComponentInspector::Update(const UpdateInfo &, continue; } - if (typeId == components::Light::typeId) - { - this->SetType("light"); - continue; - } - if (typeId == components::Actor::typeId) { this->SetType("actor"); @@ -500,6 +555,7 @@ void ComponentInspector::Update(const UpdateInfo &, if (this->dataPtr->entity == this->dataPtr->worldEntity) this->dataPtr->worldName = comp->Data(); + this->dataPtr->entityName = comp->Data(); } else if (typeId == components::LightType::typeId) { @@ -528,6 +584,22 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::Light::typeId) + { + this->SetType("light"); + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + { + msgs::Light lightMsgs = convert(comp->Data()); + setData(item, lightMsgs); + } + } + else if (typeId == components::Physics::typeId) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); @@ -735,6 +807,90 @@ void ComponentInspector::OnPose(double _x, double _y, double _z, double _roll, this->dataPtr->node.Request(poseCmdService, req, cb); } +///////////////////////////////////////////////// +void ComponentInspector::OnLight( + double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, + double _rDiffuse, double _gDiffuse, double _bDiffuse, double _aDiffuse, + double _attRange, double _attLinear, double _attConstant, + double _attQuadratic, bool _castShadows, double _directionX, + double _directionY, double _directionZ, double _innerAngle, + double _outerAngle, double _falloff, int _type) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting light configuration" << std::endl; + }; + + ignition::msgs::Light req; + req.set_name(this->dataPtr->entityName); + req.set_id(this->dataPtr->entity); + ignition::msgs::Set(req.mutable_diffuse(), + ignition::math::Color(_rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse)); + ignition::msgs::Set(req.mutable_specular(), + ignition::math::Color(_rSpecular, _gSpecular, _bSpecular, _aSpecular)); + req.set_range(_attRange); + req.set_attenuation_linear(_attLinear); + req.set_attenuation_constant(_attConstant); + req.set_attenuation_quadratic(_attQuadratic); + req.set_cast_shadows(_castShadows); + if (_type == 0) + req.set_type(ignition::msgs::Light::POINT); + else if (_type == 1) + req.set_type(ignition::msgs::Light::SPOT); + else + req.set_type(ignition::msgs::Light::DIRECTIONAL); + + if (_type == 1) // sdf::LightType::SPOT + { + req.set_spot_inner_angle(_innerAngle); + req.set_spot_outer_angle(_outerAngle); + req.set_spot_falloff(_falloff); + } + + // if sdf::LightType::SPOT || sdf::LightType::DIRECTIONAL + if (_type == 1 || _type == 2) + { + ignition::msgs::Set(req.mutable_direction(), + ignition::math::Vector3d(_directionX, _directionY, _directionZ)); + } + + auto lightConfigService = "/world/" + this->dataPtr->worldName + + "/light_config"; + lightConfigService = transport::TopicUtils::AsValidTopic(lightConfigService); + if (lightConfigService.empty()) + { + ignerr << "Invalid light command service topic provided" << std::endl; + return; + } + this->dataPtr->node.Request(lightConfigService, req, cb); +} + +///////////////////////////////////////////////// +void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) +{ + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting physics parameters" << std::endl; + }; + + ignition::msgs::Physics req; + req.set_max_step_size(_stepSize); + req.set_real_time_factor(_realTimeFactor); + auto physicsCmdService = "/world/" + this->dataPtr->worldName + + "/set_physics"; + physicsCmdService = transport::TopicUtils::AsValidTopic(physicsCmdService); + if (physicsCmdService.empty()) + { + ignerr << "Invalid physics command service topic provided" << std::endl; + return; + } + this->dataPtr->node.Request(physicsCmdService, req, cb); +} + ///////////////////////////////////////////////// bool ComponentInspector::NestedModel() const { diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 470c7b508f..cd6cc470b2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -28,6 +28,10 @@ #include #include +#include "ignition/gazebo/components/Physics.hh" + +#include + Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) namespace ignition @@ -69,12 +73,24 @@ namespace gazebo template<> void setData(QStandardItem *_item, const math::Pose3d &_data); + /// \brief Specialized to set light data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const msgs::Light &_data); + /// \brief Specialized to set vector data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. template<> void setData(QStandardItem *_item, const math::Vector3d &_data); + /// \brief Specialized to set Physics data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const sdf::Physics &_data); + /// \brief Specialized to set boolean data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -207,6 +223,42 @@ namespace gazebo public: Q_INVOKABLE void OnPose(double _x, double _y, double _z, double _roll, double _pitch, double _yaw); + /// \brief Callback in Qt thread when specular changes. + /// \param[in] _rSpecular specular red + /// \param[in] _gSpecular specular green + /// \param[in] _bSpecular specular blue + /// \param[in] _aSpecular specular alpha + /// \param[in] _rDiffuse Diffuse red + /// \param[in] _gDiffuse Diffuse green + /// \param[in] _bDiffuse Diffuse blue + /// \param[in] _aDiffuse Diffuse alpha + /// \param[in] _attRange Range attenuation + /// \param[in] _attLinear Linear attenuation + /// \param[in] _attConstant Constant attenuation + /// \param[in] _attQuadratic Quadratic attenuation + /// \param[in] _castShadows Specify if this light should cast shadows + /// \param[in] _directionX X direction of the light + /// \param[in] _directionY Y direction of the light + /// \param[in] _directionZ Z direction of the light + /// \param[in] _innerAngle Inner angle of the spotlight + /// \param[in] _outerAngle Outer angle of the spotlight + /// \param[in] _falloff Falloff of the spotlight + /// \param[in] _type light type + public: Q_INVOKABLE void OnLight( + double _rSpecular, double _gSpecular, double _bSpecular, + double _aSpecular, double _rDiffuse, double _gDiffuse, + double _bDiffuse, double _aDiffuse, double _attRange, + double _attLinear, double _attConstant, double _attQuadratic, + bool _castShadows, double _directionX, double _directionY, + double _directionZ, double _innerAngle, double _outerAngle, + double _falloff, int _type); + + /// \brief Callback in Qt thread when physics' properties change. + /// \param[in] _stepSize step size + /// \param[in] _realTimeFactor real time factor + public: Q_INVOKABLE void OnPhysics(double _stepSize, + double _realTimeFactor); + /// \brief Get whether the entity is a nested model or not /// \return True if the entity is a nested model, false otherwise public: Q_INVOKABLE bool NestedModel() const; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index aa058cae4f..1c3ed07414 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -85,6 +85,28 @@ Rectangle { ComponentInspector.OnPose(_x, _y, _z, _roll, _pitch, _yaw) } + /** + * Forward light changes to C++ + */ + function onLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular, + _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, + _attRange, _attLinear, _attConstant, _attQuadratic, + _castShadows, _directionX, _directionY, _directionZ, + _innerAngle, _outerAngle, _falloff, _type) { + ComponentInspector.OnLight(_rSpecular, _gSpecular, _bSpecular, _aSpecular, + _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, + _attRange, _attLinear, _attConstant, _attQuadratic, + _castShadows, _directionX, _directionY, _directionZ, + _innerAngle, _outerAngle, _falloff, _type) + } + + /* + * Forward physics changes to C++ + */ + function onPhysics(_stepSize, _realTimeFactor) { + ComponentInspector.OnPhysics(_stepSize, _realTimeFactor) + } + Rectangle { id: header height: lockButton.height diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 43efec19d1..1996720cbc 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -2,7 +2,9 @@ Boolean.qml ComponentInspector.qml + Light.qml NoData.qml + Physics.qml Pose3d.qml String.qml TypeHeader.qml diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml new file mode 100644 index 0000000000..68314d7053 --- /dev/null +++ b/src/gui/plugins/component_inspector/Light.qml @@ -0,0 +1,1099 @@ +/* + * Copyright (C) 2020 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying light information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + property int iconWidth: 20 + property int iconHeight: 20 + + // Loaded item for specular red + property var rSpecularItem: {} + + // Loaded item for specular green + property var gSpecularItem: {} + + // Loaded item for specular blue + property var bSpecularItem: {} + + // Loaded item for specular alpha + property var aSpecularItem: {} + + // Loaded item for diffuse red + property var rDiffuseItem: {} + + // Loaded item for diffuse green + property var gDiffuseItem: {} + + // Loaded item for diffuse blue + property var bDiffuseItem: {} + + // Loaded item for diffuse alpha + property var aDiffuseItem: {} + + // Loaded item for attenuation range + property var attRangeItem: {} + + // Loaded item for attenuation linear + property var attLinearItem: {} + + // Loaded item for attenuation constant + property var attConstantItem: {} + + // Loaded item for attenuation quadratic + property var attQuadraticItem: {} + + // Loaded item for cast shadows + property var castShadowsItem: {} + + // Loaded item for direction X (spotlight or directional) + property var directionXItem: {} + + // Loaded item for direction Y (spotlight or directional) + property var directionYItem: {} + + // Loaded item for direction Z (spotlight or directional) + property var directionZItem: {} + + // Loaded item for inner angle (spotlight) + property var innerAngleItem: {} + + // Loaded item for inner angle (spotlight) + property var outerAngleItem: {} + + // Loaded item for falloff (spotlight) + property var falloffItem: {} + + // Send new light data to C++ + function sendLight() { + // TODO(anyone) There's a loss of precision when these values get to C++ + componentInspector.onLight( + rSpecularItem.value, + gSpecularItem.value, + bSpecularItem.value, + aSpecularItem.value, + rDiffuseItem.value, + gDiffuseItem.value, + bDiffuseItem.value, + aDiffuseItem.value, + attRangeItem.value, + attLinearItem.value, + attConstantItem.value, + attQuadraticItem.value, + castShadowsItem.checked, + directionXItem.value, + directionYItem.value, + directionZItem.value, + innerAngleItem.value, + outerAngleItem.value, + falloffItem.value, + model.data[19] + ); + } + + FontMetrics { + id: fontMetrics + font.family: "Roboto" + } + + /** + * Used to create a spin box + */ + Component { + id: sliderZeroOne + Slider { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + from: 0.0 + to: 1.0 + onValueChanged: { + if (hovered){ + sendLight() + } + } + } + } + + Component { + id: ignSwitch + Switch { + id: booleanSwitch + checked: numberValue + enabled: true + onToggled: { + sendLight() + } + } + } + + Component { + id: spinZeroMax + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: 0 + maximumValue: 1000000 + decimals: 6 + onEditingFinished: { + sendLight() + } + } + } + Component { + id: spinNoLimit + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: -100000 + maximumValue: 100000 + decimals: 6 + onEditingFinished: { + sendLight() + } + } + } + + Component { + id: plotIcon + Image { + property string componentInfo: "" + source: "plottable_icon.svg" + anchors.top: parent.top + anchors.left: parent.left + + Drag.mimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + componentInfo + "," + model.shortName + } + Drag.dragType: Drag.Automatic + Drag.supportedActions : Qt.CopyAction + Drag.active: dragMouse.drag.active + // a point to drag from + Drag.hotSpot.x: 0 + Drag.hotSpot.y: y + MouseArea { + id: dragMouse + anchors.fill: parent + drag.target: (model === null) ? null : parent + onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) + onReleased: parent.Drag.drop(); + cursorShape: Qt.DragCopyCursor + } + } + } + + Column { + anchors.fill: parent + + // Header + Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + RowLayout { + anchors.fill: parent + Item { + width: margin + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: content.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + content.show = !content.show + } + onEntered: { + header.color = highlightColor + } + onExited: { + header.color = "transparent" + } + } + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? grid.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + ColumnLayout { + id: grid + width: parent.width + + RowLayout { + Layout.alignment : Qt.AlignLeft + Text { + text: " Specular" + color: "dimgrey" + width: margin + indentation + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: rSpecularText.width + indentation*3 + Loader { + id: loaderSpecularR + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderSpecularR.item.componentInfo = "specularR" + + Text { + id : rSpecularText + text: ' R' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: rSpecularLoader + anchors.fill: parent + property double numberValue: model.data[0] + sourceComponent: sliderZeroOne + onLoaded: { + rSpecularItem = rSpecularLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: gSpecularText.width + indentation*3 + Loader { + id: loaderSpecularG + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderSpecularG.item.componentInfo = "specularG" + + Text { + id : gSpecularText + text: ' G' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: gSpecularLoader + anchors.fill: parent + property double numberValue: model.data[1] + sourceComponent: sliderZeroOne + onLoaded: { + gSpecularItem = gSpecularLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: bSpecularText.width + indentation*3 + Loader { + id: loaderSpecularB + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderSpecularB.item.componentInfo = "specularB" + + Text { + id : bSpecularText + text: ' B' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: bSpecularLoader + anchors.fill: parent + property double numberValue: model.data[2] + sourceComponent: sliderZeroOne + onLoaded: { + bSpecularItem = bSpecularLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: aSpecularText.width + indentation*3 + Loader { + id: loaderSpecularA + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderSpecularA.item.componentInfo = "specularA" + + Text { + id : aSpecularText + text: ' A' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: aSpecularLoader + anchors.fill: parent + property double numberValue: model.data[3] + sourceComponent: sliderZeroOne + onLoaded: { + aSpecularItem = aSpecularLoader.item + } + } + } + } + RowLayout { + Layout.alignment: Qt.AlignHCenter + Button { + Layout.alignment: Qt.AlignHCenter + id: specularColor + text: qsTr("Specular Color") + onClicked: colorDialog.open() + ColorDialog { + id: colorDialog + title: "Choose a specular color" + visible: false + onAccepted: { + rSpecularLoader.item.value = colorDialog.color.r + gSpecularLoader.item.value = colorDialog.color.g + bSpecularLoader.item.value = colorDialog.color.b + aSpecularLoader.item.value = colorDialog.color.a + sendLight() + colorDialog.close() + } + onRejected: { + colorDialog.close() + } + } + } + } + RowLayout { + Text { + Layout.columnSpan: 6 + text: " Diffuse" + color: "dimgrey" + width: margin + indentation + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: rDiffuseText.width + indentation*3 + Loader { + id: loaderDiffuseR + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDiffuseR.item.componentInfo = "diffuseR" + + Text { + id : rDiffuseText + text: ' R' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: rDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[4] + sourceComponent: sliderZeroOne + onLoaded: { + rDiffuseItem = rDiffuseLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: gDiffuseText.width + indentation*3 + Loader { + id: loaderDiffuseG + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDiffuseG.item.componentInfo = "diffuseG" + + Text { + id : gDiffuseText + text: ' G' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: gDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[5] + sourceComponent: sliderZeroOne + onLoaded: { + gDiffuseItem = gDiffuseLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: bDiffuseText.width + indentation*3 + Loader { + id: loaderDiffuseB + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDiffuseB.item.componentInfo = "diffuseB" + + Text { + id : bDiffuseText + text: ' B' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: bDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[6] + sourceComponent: sliderZeroOne + onLoaded: { + bDiffuseItem = bDiffuseLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: aDiffuseText.width + indentation*3 + Loader { + id: loaderDiffuseA + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDiffuseA.item.componentInfo = "diffuseA" + + Text { + id : aDiffuseText + text: ' A' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: aDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[7] + sourceComponent: sliderZeroOne + onLoaded: { + aDiffuseItem = aDiffuseLoader.item + } + } + } + } + RowLayout { + Layout.alignment: Qt.AlignHCenter + Button { + Layout.alignment: Qt.AlignHCenter + id: diffuseColor + text: qsTr("Diffuse Color") + onClicked: colorDialogDiffuse.open() + ColorDialog { + id: colorDialogDiffuse + title: "Choose a diffuse color" + visible: false + onAccepted: { + rDiffuseLoader.item.value = colorDialogDiffuse.color.r + gDiffuseLoader.item.value = colorDialogDiffuse.color.g + bDiffuseLoader.item.value = colorDialogDiffuse.color.b + aDiffuseLoader.item.value = colorDialogDiffuse.color.a + sendLight() + colorDialogDiffuse.close() + } + onRejected: { + colorDialogDiffuse.close() + } + } + } + } + RowLayout { + Text { + Layout.columnSpan: 6 + text: " Attenuation" + color: "dimgrey" + width: margin + indentation + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: attRangeText.width + indentation*3 + Loader { + id: loaderAttRange + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderAttRange.item.componentInfo = "attRange" + + Text { + id : attRangeText + text: ' Range' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: attRangeLoader + anchors.fill: parent + property double numberValue: model.data[8] + sourceComponent: spinZeroMax + onLoaded: { + attRangeItem = attRangeLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: attLinearText.width + indentation*3 + Loader { + id: loaderAttLinear + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderAttLinear.item.componentInfo = "attLinear" + + Text { + id : attLinearText + text: ' Linear' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: attLinearLoader + anchors.fill: parent + property double numberValue: model.data[9] + sourceComponent: sliderZeroOne + onLoaded: { + attLinearItem = attLinearLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: attConstantText.width + indentation*3 + Loader { + id: loaderAttConstant + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderAttConstant.item.componentInfo = "attConstant" + + Text { + id : attConstantText + text: ' Constant' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: attConstantLoader + anchors.fill: parent + property double numberValue: model.data[10] + sourceComponent: sliderZeroOne + onLoaded: { + attConstantItem = attConstantLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: attQuadraticText.width + indentation*3 + Loader { + id: loaderAttQuadratic + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderAttQuadratic.item.componentInfo = "attQuadratic" + + Text { + id : attQuadraticText + text: ' Quadratic' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: attQuadraticLoader + anchors.fill: parent + property double numberValue: model.data[11] + sourceComponent: spinZeroMax + onLoaded: { + attQuadraticItem = attQuadraticLoader.item + } + } + } + } + RowLayout { + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: castShadowsText.width + indentation*3 + Loader { + id: loaderCastShadows + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderCastShadows.item.componentInfo = "castshadows" + + Text { + id : castShadowsText + text: ' Cast shadows' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + + Loader { + id: castShadowLoader + anchors.fill: parent + property double numberValue: model.data[12] + sourceComponent: ignSwitch + onLoaded: { + castShadowsItem = castShadowLoader.item + } + } + } + } + RowLayout { + Text { + visible: model.data[19] === 1 || model.data[19] === 2 + Layout.columnSpan: 6 + text: " Direction" + color: "dimgrey" + width: margin + indentation + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 || model.data[19] === 2 + color: "transparent" + height: 40 + Layout.preferredWidth: xDirectionText.width + indentation*3 + Loader { + id: loaderDirectionX + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDirectionX.item.componentInfo = "directionX" + + Text { + visible: model.data[19] === 1 || model.data[19] === 2 + id : xDirectionText + text: ' X:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 || model.data[19] === 2 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: xDirectionLoader + anchors.fill: parent + property double numberValue: model.data[13] + sourceComponent: spinNoLimit + onLoaded: { + directionXItem = xDirectionLoader.item + } + } + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 || model.data[19] === 2 + color: "transparent" + height: 40 + Layout.preferredWidth: yDirectionText.width + indentation*3 + Loader { + id: loaderDirectionY + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDirectionY.item.componentInfo = "directionY" + + Text { + visible: model.data[19] === 1 || model.data[19] === 2 + id : yDirectionText + text: ' Y:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 || model.data[19] === 2 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: yDirectionLoader + anchors.fill: parent + property double numberValue: model.data[14] + sourceComponent: spinNoLimit + onLoaded: { + directionYItem = yDirectionLoader.item + } + } + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 || model.data[19] === 2 + color: "transparent" + height: 40 + Layout.preferredWidth: zDirectionText.width + indentation*3 + Loader { + id: loaderDirectionZ + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderDirectionZ.item.componentInfo = "directionZ" + + Text { + visible: model.data[19] === 1 || model.data[19] === 2 + id : zDirectionText + text: ' Z:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 || model.data[19] === 2 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: zDirectionLoader + anchors.fill: parent + property double numberValue: model.data[15] + sourceComponent: spinNoLimit + onLoaded: { + directionZItem = zDirectionLoader.item + } + } + } + } + RowLayout { + Text { + visible: model.data[19] === 1 + Layout.columnSpan: 6 + text: " Spot features" + color: "dimgrey" + width: margin + indentation + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 + color: "transparent" + height: 40 + Layout.preferredWidth: innerAngleText.width + indentation*3 + Loader { + id: loaderInnerAngle + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderInnerAngle.item.componentInfo = "innerAngle" + + Text { + visible: model.data[19] === 1 + id : innerAngleText + text: ' Inner Angle:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: innerAngleLoader + anchors.fill: parent + property double numberValue: model.data[16] + sourceComponent: spinNoLimit + onLoaded: { + innerAngleItem = innerAngleLoader.item + } + } + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 + color: "transparent" + height: 40 + Layout.preferredWidth: outerAngleText.width + indentation*3 + Loader { + id: loaderOuterAngle + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderOuterAngle.item.componentInfo = "outerAngle" + + Text { + visible: model.data[19] === 1 + id : outerAngleText + text: ' Outer angle:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: outerAngleLoader + anchors.fill: parent + property double numberValue: model.data[17] + sourceComponent: spinNoLimit + onLoaded: { + outerAngleItem = outerAngleLoader.item + } + } + } + } + RowLayout { + Rectangle { + visible: model.data[19] === 1 + color: "transparent" + height: 40 + Layout.preferredWidth: fallOffText.width + indentation*3 + Loader { + id: loaderFallOff + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderFallOff.item.componentInfo = "falloff" + + Text { + visible: model.data[19] === 1 + id : fallOffText + text: ' Falloff:' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + visible: model.data[19] === 1 + Layout.fillWidth: true + height: 40 + Layout.columnSpan: 4 + Loader { + id: fallOffLoader + anchors.fill: parent + property double numberValue: model.data[18] + sourceComponent: spinZeroMax + onLoaded: { + falloffItem = fallOffLoader.item + } + } + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/Physics.qml b/src/gui/plugins/component_inspector/Physics.qml new file mode 100644 index 0000000000..71f0a34fda --- /dev/null +++ b/src/gui/plugins/component_inspector/Physics.qml @@ -0,0 +1,263 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" +import "qrc:/qml" + +// Item displaying physics information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Minimum parameter value, step size and RTF must be strictly positive + property double minPhysParam: 0.000001 + + // Maximum parameter value + property double maxPhysParam: 100000 + + // Horizontal margins + property int margin: 5 + + property int iconWidth: 20 + property int iconHeight: 20 + + // Loaded item for physics step size + property var stepSizeItem: {} + + // Loaded item for real time factor + property var realTimeFactorItem: {} + + // Send new physics data to C++ + function sendPhysics() { + // TODO(anyone) There's a loss of precision when these values get to C++ + componentInspector.onPhysics( + stepSizeItem.value, + realTimeFactorItem.value + ); + } + + FontMetrics { + id: fontMetrics + font.family: "Roboto" + } + + /** + * Used to create a spin box + */ + + Component { + id: writablePositiveNumber + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: minPhysParam + maximumValue: maxPhysParam + decimals: 6 + stepSize: 0.001 + onEditingFinished: { + sendPhysics() + } + } + } + + Component { + id: plotIcon + Image { + property string componentInfo: "" + source: "plottable_icon.svg" + anchors.top: parent.top + anchors.left: parent.left + + Drag.mimeData: { "text/plain" : (model === null) ? "" : + "Component," + model.entity + "," + model.typeId + "," + + model.dataType + "," + componentInfo + "," + model.shortName + } + Drag.dragType: Drag.Automatic + Drag.supportedActions : Qt.CopyAction + Drag.active: dragMouse.drag.active + // a point to drag from + Drag.hotSpot.x: 0 + Drag.hotSpot.y: y + MouseArea { + id: dragMouse + anchors.fill: parent + drag.target: (model === null) ? null : parent + onPressed: parent.grabToImage(function(result) {parent.Drag.imageSource = result.url }) + onReleased: parent.Drag.drop(); + cursorShape: Qt.DragCopyCursor + } + } + } + + Column { + anchors.fill: parent + + // Header + Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + RowLayout { + anchors.fill: parent + Item { + width: margin + } + Image { + id: icon + sourceSize.height: indentation + sourceSize.width: indentation + fillMode: Image.Pad + Layout.alignment : Qt.AlignVCenter + source: content.show ? + "qrc:/Gazebo/images/minus.png" : "qrc:/Gazebo/images/plus.png" + } + TypeHeader { + id: typeHeader + } + Item { + Layout.fillWidth: true + } + } + MouseArea { + anchors.fill: parent + hoverEnabled: true + cursorShape: Qt.PointingHandCursor + onClicked: { + content.show = !content.show + } + onEntered: { + header.color = highlightColor + } + onExited: { + header.color = "transparent" + } + } + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? grid.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + GridLayout { + id: grid + width: parent.width + columns: 3 + + // Left spacer + Item { + Layout.rowSpan: 3 + width: margin + indentation + } + + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: stepSizeText.width + indentation*3 + Loader { + id: loaderStepSize + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderStepSize.item.componentInfo = "stepSize" + + Text { + id : stepSizeText + text: ' Step Size (s)' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: stepSizeLoader + anchors.fill: parent + property double numberValue: model.data[0] + sourceComponent: writablePositiveNumber + onLoaded: { + stepSizeItem = stepSizeLoader.item + } + } + } + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: realTimeFactorText.width + indentation*3 + Loader { + id: loaderRealTimeFactor + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderRealTimeFactor.item.componentInfo = "realTimeFactor" + + Text { + id : realTimeFactorText + text: ' Real time factor' + leftPadding: 5 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + Item { + Layout.fillWidth: true + height: 40 + Loader { + id: realTimeFactorLoader + anchors.fill: parent + property double numberValue: model.data[1] + sourceComponent: writablePositiveNumber + onLoaded: { + realTimeFactorItem = realTimeFactorLoader.item + } + } + } + } + } + } +} diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index 5bd42b2510..e76cbd8cf1 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -47,6 +47,9 @@ namespace ignition::gazebo /// \brief Remove service name public: std::string removeService; + /// \brief View collisions service name + public: std::string viewCollisionsService; + /// \brief Name of world. public: std::string worldName; }; @@ -75,6 +78,9 @@ EntityContextMenu::EntityContextMenu() // For remove service requests this->dataPtr->removeService = "/world/default/remove"; + + // For view collisions service requests + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; } ///////////////////////////////////////////////// @@ -146,6 +152,12 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->followService, req, cb); } + else if (request == "view_collisions") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewCollisionsService, req, cb); + } else { ignwarn << "Unknown request [" << request << "]" << std::endl; diff --git a/src/gui/plugins/modules/EntityContextMenu.qml b/src/gui/plugins/modules/EntityContextMenu.qml index 9e927e2356..bb2f4e02be 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,6 +37,40 @@ Item { text: "Remove" onTriggered: context.OnRemove(context.entity, context.type) } + // // cascading submenu only works in Qt 5.10+ on focal + // Menu { + // id: viewSubmenu + // title: "View" + // MenuItem { + // id: viewCollisionsMenu + // text: "Collisions" + // onTriggered: context.OnRequest("view_collisions", context.entity) + // } + // } + // workaround for getting submenu's to work on bionic (< Qt 5.10) + MenuItem { + id: viewSubmenu + text: "View >" + MouseArea { + id: viewSubMouseArea + anchors.fill: parent + hoverEnabled: true + onEntered: secondMenu.open() + } + } + } + Menu { + id: secondMenu + x: menu.x + menu.width + y: menu.y + viewSubmenu.y + MenuItem { + id: viewCollisionsMenu + text: "Collisions" + onTriggered: { + menu.close() + context.OnRequest("view_collisions", context.entity) + } + } } function open(_entity, _type, _x, _y) { @@ -47,6 +81,7 @@ Item { moveToMenu.enabled = false followMenu.enabled = false removeMenu.enabled = false + viewCollisionsMenu.enabled = false; // enable / disable menu items if (context.type == "model" || context.type == "link" || @@ -62,6 +97,11 @@ Item { removeMenu.enabled = true } + if (context.type == "model" || context.type == "link") + { + viewCollisionsMenu.enabled = true; + } + menu.open() } @@ -71,5 +111,3 @@ Item { property string type } } - - diff --git a/src/gui/plugins/plotting/Plotting.cc b/src/gui/plugins/plotting/Plotting.cc index ef20533382..9a78d4de15 100644 --- a/src/gui/plugins/plotting/Plotting.cc +++ b/src/gui/plugins/plotting/Plotting.cc @@ -18,16 +18,19 @@ #include "Plotting.hh" #include + #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/CastShadows.hh" #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Gravity.hh" +#include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/LinearVelocitySeed.hh" #include "ignition/gazebo/components/MagneticField.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/Static.hh" @@ -94,8 +97,35 @@ PlotComponent::PlotComponent(const std::string &_type, this->dataPtr->data["pitch"] = std::make_shared(); this->dataPtr->data["yaw"] = std::make_shared(); } + else if (_type == "Light") + { + this->dataPtr->data["diffuseR"] = std::make_shared(); + this->dataPtr->data["diffuseG"] = std::make_shared(); + this->dataPtr->data["diffuseB"] = std::make_shared(); + this->dataPtr->data["diffuseA"] = std::make_shared(); + this->dataPtr->data["specularR"] = std::make_shared(); + this->dataPtr->data["specularG"] = std::make_shared(); + this->dataPtr->data["specularB"] = std::make_shared(); + this->dataPtr->data["specularA"] = std::make_shared(); + this->dataPtr->data["attRange"] = std::make_shared(); + this->dataPtr->data["attConstant"] = std::make_shared(); + this->dataPtr->data["attLinear"] = std::make_shared(); + this->dataPtr->data["attQuadratic"] = std::make_shared(); + this->dataPtr->data["castshadows"] = std::make_shared(); + this->dataPtr->data["directionX"] = std::make_shared(); + this->dataPtr->data["directionY"] = std::make_shared(); + this->dataPtr->data["directionZ"] = std::make_shared(); + this->dataPtr->data["innerAngle"] = std::make_shared(); + this->dataPtr->data["outerAngle"] = std::make_shared(); + this->dataPtr->data["falloff"] = std::make_shared(); + } else if (_type == "double") this->dataPtr->data["value"] = std::make_shared(); + else if (_type == "Physics") + { + this->dataPtr->data["stepSize"] = std::make_shared(); + this->dataPtr->data["realTimeFactor"] = std::make_shared(); + } else ignwarn << "Invalid Plot Component Type:" << _type << std::endl; } @@ -203,6 +233,57 @@ void Plotting::SetData(std::string _Id, const ignition::math::Vector3d &_vector) this->dataPtr->components[_Id]->SetAttributeValue("z", _vector.Z()); } +void Plotting::SetData(std::string _Id, const ignition::msgs::Light &_light) +{ + if (_light.has_specular()) + { + this->dataPtr->components[_Id]->SetAttributeValue("specularR", + _light.specular().r()); + this->dataPtr->components[_Id]->SetAttributeValue("specularG", + _light.specular().g()); + this->dataPtr->components[_Id]->SetAttributeValue("specularB", + _light.specular().b()); + this->dataPtr->components[_Id]->SetAttributeValue("specularA", + _light.specular().a()); + } + if (_light.has_diffuse()) + { + this->dataPtr->components[_Id]->SetAttributeValue("diffuseR", + _light.diffuse().r()); + this->dataPtr->components[_Id]->SetAttributeValue("diffuseG", + _light.diffuse().g()); + this->dataPtr->components[_Id]->SetAttributeValue("diffuseB", + _light.diffuse().b()); + this->dataPtr->components[_Id]->SetAttributeValue("diffuseA", + _light.diffuse().a()); + } + this->dataPtr->components[_Id]->SetAttributeValue("attRange", + _light.range()); + this->dataPtr->components[_Id]->SetAttributeValue("attLinear", + _light.attenuation_linear()); + this->dataPtr->components[_Id]->SetAttributeValue("attConstant", + _light.attenuation_constant()); + this->dataPtr->components[_Id]->SetAttributeValue("attQuadratic", + _light.attenuation_quadratic()); + this->dataPtr->components[_Id]->SetAttributeValue("castshadows", + _light.cast_shadows()); + if (_light.has_direction()) + { + this->dataPtr->components[_Id]->SetAttributeValue("directionX", + _light.direction().x()); + this->dataPtr->components[_Id]->SetAttributeValue("directionY", + _light.direction().y()); + this->dataPtr->components[_Id]->SetAttributeValue("directionZ", + _light.direction().z()); + } + this->dataPtr->components[_Id]->SetAttributeValue("innerAngle", + _light.spot_inner_angle()); + this->dataPtr->components[_Id]->SetAttributeValue("outerAngle", + _light.spot_outer_angle()); + this->dataPtr->components[_Id]->SetAttributeValue("falloff", + _light.spot_falloff()); +} + ////////////////////////////////////////////////// void Plotting::SetData(std::string _Id, const ignition::math::Pose3d &_pose) { @@ -215,6 +296,15 @@ void Plotting::SetData(std::string _Id, const ignition::math::Pose3d &_pose) this->dataPtr->components[_Id]->SetAttributeValue("yaw", _pose.Rot().Yaw()); } +////////////////////////////////////////////////// +void Plotting::SetData(std::string _Id, const sdf::Physics &_physics) +{ + this->dataPtr->components[_Id]->SetAttributeValue("stepSize", + _physics.MaxStepSize()); + this->dataPtr->components[_Id]->SetAttributeValue("realTimeFactor", + _physics.RealTimeFactor()); +} + ////////////////////////////////////////////////// void Plotting::SetData(std::string _Id, const double &_value) { @@ -325,6 +415,12 @@ void Plotting ::Update(const ignition::gazebo::UpdateInfo &_info, if (comp) this->SetData(component.first, comp->Data()); } + else if (typeId == components::Physics::typeId) + { + auto comp = _ecm.Component(entity); + if (comp) + this->SetData(component.first, comp->Data()); + } else if (typeId == components::Pose::typeId) { auto comp = _ecm.Component(entity); @@ -382,6 +478,16 @@ void Plotting ::Update(const ignition::gazebo::UpdateInfo &_info, if (comp) this->SetData(component.first, comp->Data()); } + else if (typeId == components::Light::typeId) + { + auto comp = _ecm.Component(entity); + if (comp) + { + ignition::msgs::Light lightMsgs = + convert(comp->Data()); + this->SetData(component.first, lightMsgs); + } + } for (auto attribute : component.second->Data()) { diff --git a/src/gui/plugins/plotting/Plotting.hh b/src/gui/plugins/plotting/Plotting.hh index bf9c54c69b..534d9a5008 100644 --- a/src/gui/plugins/plotting/Plotting.hh +++ b/src/gui/plugins/plotting/Plotting.hh @@ -23,6 +23,9 @@ #include #include #include +#include + +#include "sdf/Physics.hh" #include #include @@ -113,12 +116,24 @@ class Plotting : public ignition::gazebo::GuiSystem public: void SetData(std::string _Id, const ignition::math::Vector3d &_vector); + /// \brief Set the Component data of giving id to the giving light + /// \param [in] _Id Component Key of the components map + /// \param [in] _light Vector Data to be set to the component + public: void SetData(std::string _Id, + const msgs::Light &_light); + /// \brief Set the Component data of giving id to the giving pose /// \param [in] _Id Component Key of the components map /// \param [in] _pose Position Data to be set to the component public: void SetData(std::string _Id, const ignition::math::Pose3d &_pose); + /// \brief Set the Component data of giving id to the giving + /// physics properties + /// \param [in] _Id Component Key of the components map + /// \param [in] _value physics Data to be set to the component + public: void SetData(std::string _Id, const sdf::Physics &_physics); + /// \brief Set the Component data of giving id to the giving number /// \param [in] _Id Component Key of the components map /// \param [in] _value double Data to be set to the component diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index d365dc8236..80c39d60b9 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -233,6 +233,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Helper object to move user camera public: MoveToHelper moveToHelper; + /// \brief Target to view collisions + public: std::string viewCollisionsTarget; + /// \brief Helper object to select entities. Only the latest selection /// request is kept. public: SelectionHelper selectionHelper; @@ -431,6 +434,9 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief mutex to protect the render condition variable /// Used when recording in lockstep mode. public: std::mutex renderMutex; + + /// \brief View collisions service + public: std::string viewCollisionsService; }; } } @@ -805,6 +811,32 @@ void IgnRenderer::Render() } } + // View collisions + { + IGN_PROFILE("IgnRenderer::Render ViewCollisions"); + if (!this->dataPtr->viewCollisionsTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewCollisionsTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewCollisions(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewCollisionsTarget + << "] to view collisions" << std::endl; + } + + this->dataPtr->viewCollisionsTarget.clear(); + } + } + if (ignition::gui::App()) { ignition::gui::events::Render event; @@ -1974,6 +2006,13 @@ void IgnRenderer::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->moveToPoseValue = _pose; } +///////////////////////////////////////////////// +void IgnRenderer::SetViewCollisionsTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewCollisionsTarget = _target; +} + ///////////////////////////////////////////////// void IgnRenderer::SetFollowPGain(double _gain) { @@ -2700,6 +2739,13 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Camera pose topic advertised on [" << this->dataPtr->cameraPoseTopic << "]" << std::endl; + // view collisions service + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; + this->dataPtr->node.Advertise(this->dataPtr->viewCollisionsService, + &Scene3D::OnViewCollisions, this); + ignmsg << "View collisions service on [" + << this->dataPtr->viewCollisionsService << "]" << std::endl; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); ignition::gui::App()->findChild< @@ -2749,6 +2795,7 @@ void Scene3D::Update(const UpdateInfo &_info, msgs::Pose poseMsg = msgs::Convert(renderWindow->CameraPose()); this->dataPtr->cameraPosePub.Publish(poseMsg); } + this->dataPtr->renderUtil->UpdateECM(_info, _ecm); this->dataPtr->renderUtil->UpdateFromECM(_info, _ecm); // check if video recording is enabled and if we need to lock step @@ -2852,6 +2899,18 @@ bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) return true; } +///////////////////////////////////////////////// +bool Scene3D::OnViewCollisions(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewCollisionsTarget(_msg.data()); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// void Scene3D::OnHovered(int _mouseX, int _mouseY) { @@ -3109,6 +3168,12 @@ void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->renderThread->ignRenderer.SetMoveToPose(_pose); } +///////////////////////////////////////////////// +void RenderWindowItem::SetViewCollisionsTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewCollisionsTarget(_target); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetFollowPGain(double _gain) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index 07143e40dd..3f51fde8dc 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -154,6 +154,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res); + /// \brief Callback for view collisions request + /// \param[in] _msg Request message to set the target to view collisions + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewCollisions(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -245,6 +252,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The world pose to set the camera to. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \brief View collisions of the specified target + /// \param[in] _target Target to view collisions + public: void SetViewCollisionsTarget(const std::string &_target); + /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); @@ -587,6 +598,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _pose The new camera pose in the world frame. public: void SetMoveToPose(const math::Pose3d &_pose); + /// \brief View collisions of the specified target + /// \param[in] _target Target to view collisions + public: void SetViewCollisionsTarget(const std::string &_target); + /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); diff --git a/src/gui/resources/GazeboDrawer.qml b/src/gui/resources/GazeboDrawer.qml index 1974f87176..dc1ffa922f 100644 --- a/src/gui/resources/GazeboDrawer.qml +++ b/src/gui/resources/GazeboDrawer.qml @@ -54,6 +54,9 @@ Rectangle { case "saveWorldAs": sdfGenConfigDialog.open(); break + case "aboutDialog": + aboutDialog.open(); + break // Forward others to default drawer default: parent.onAction(_action); @@ -94,6 +97,10 @@ Rectangle { title: "Style settings" actionElement: "styleSettings" } + ListElement { + title: "About" + actionElement: "aboutDialog" + } ListElement { title: "Quit" actionElement: "close" @@ -134,6 +141,34 @@ Rectangle { } } + /** + * About dialog + */ + Dialog { + id: aboutDialog + title: "Ignition Gazebo" + + modal: true + focus: true + parent: ApplicationWindow.overlay + width: parent.width / 3 > 500 ? 500 : parent.width / 3 + x: (parent.width - width) / 2 + y: (parent.height - height) / 2 + closePolicy: Popup.CloseOnEscape + standardButtons: StandardButton.Ok + + Text { + anchors.fill: parent + id: aboutMessage + wrapMode: Text.Wrap + verticalAlignment: Text.AlignVCenter + color: Material.theme == Material.Light ? "black" : "white" + textFormat: Text.RichText + text: AboutDialogHandler.getVersionInformation() + onLinkActivated: AboutDialogHandler.openURL(link) + } + } + /** * Dialog with configurations for SDF generation */ diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 8a1732bf12..be9e543255 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -16,10 +16,14 @@ */ #include +#include +#include #include +#include #include #include +#include #include #include #include @@ -38,6 +42,8 @@ #include #include +#include + #include #include #include @@ -46,19 +52,25 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Light.hh" +#include "ignition/gazebo/components/LightCmd.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Material.hh" #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/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visibility.hh" @@ -162,6 +174,17 @@ class ignition::gazebo::RenderUtilPrivate public: std::vector> newSensors; + /// \brief New particle emitter to be created. The elements in the tuple are: + /// [0] entity id, [1], particle emitter, [2] parent entity id + public: std::vector> + newParticleEmitters; + + /// \brief New particle emitter commands to be requested. + /// The map key and value are: entity id of the particle emitter to + /// update, and particle emitter msg + public: std::unordered_map + newParticleEmittersCmds; + /// \brief Map of ids of entites to be removed and sim iteration when the /// remove request is received public: std::unordered_map removeEntities; @@ -169,12 +192,29 @@ class ignition::gazebo::RenderUtilPrivate /// \brief A map of entity ids and pose updates. public: std::unordered_map entityPoses; + /// \brief A map of entity ids and light updates. + public: std::unordered_map entityLights; + + /// \brief A map of entity ids and light updates. + public: std::vector entityLightsCmdToDelete; + /// \brief A map of entity ids and actor transforms. public: std::map> actorTransforms; - /// \brief A map of entity ids and temperature - public: std::map entityTemp; + /// \brief A map of entity ids and temperature data. + /// The value of this map (tuple) represents either a single (uniform) + /// temperature, or a heat signature with a min/max temperature. If the string + /// in the tuple is empty, then this entity has a uniform temperature across + /// its surface, and this uniform temperature is stored in the first float of + /// the tuple (the second float and string are unused for uniform temperature + /// entities). If the string in the tuple is not empty, then the string + /// represents the entity's heat signature (a path to a heat signature texture + /// file), and the floats represent the min/max temperatures of the heat + /// signature, respectively. + /// + /// All temperatures are in Kelvin. + public: std::map> entityTemp; /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -206,6 +246,36 @@ class ignition::gazebo::RenderUtilPrivate public: std::function createSensorCb; + /// \brief Light equality comparison function. + public: std::function + lightEql { [](const sdf::Light &_a, const sdf::Light &_b) + { + return + _a.Type() == _b.Type() && + _a.Name() == _b.Name() && + _a.Diffuse() == _b.Diffuse() && + _a.Specular() == _b.Specular() && + math::equal( + _a.AttenuationRange(), _b.AttenuationRange(), 1e-6) && + math::equal( + _a.LinearAttenuationFactor(), + _b.LinearAttenuationFactor(), + 1e-6) && + math::equal( + _a.ConstantAttenuationFactor(), + _b.ConstantAttenuationFactor(), + 1e-6) && + math::equal( + _a.QuadraticAttenuationFactor(), + _b.QuadraticAttenuationFactor(), + 1e-6) && + _a.CastShadows() == _b.CastShadows() && + _a.Direction() == _b.Direction() && + _a.SpotInnerAngle() == _b.SpotInnerAngle() && + _a.SpotOuterAngle() == _b.SpotOuterAngle() && + math::equal(_a.SpotFalloff(), _b.SpotFalloff(), 1e-6); + }}; + /// \brief Callback function for removing sensors. /// The function arg is the entity id public: std::function removeSensorCb; @@ -226,6 +296,36 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Restore a highlighted node to normal. /// \param[in] _node Node to be restored. public: void LowlightNode(const rendering::NodePtr &_node); + + /// \brief New collisions to be created + public: std::vector newCollisions; + + /// \brief Finds the links (collision parent) that are used to create child + /// collision visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindCollisionLinks(const EntityComponentManager &_ecm); + + /// \brief A list of links used to create new collision visuals + public: std::vector newCollisionLinks; + + /// \brief A map of collision entity ids and their SDF DOM + public: std::map entityCollisions; + + /// \brief A map of model entities and their corresponding children links + public: std::map> modelToLinkEntities; + + /// \brief A map of link entities and their corresponding children collisions + public: std::map> linkToCollisionEntities; + + /// \brief A map of created collision entities and if they are currently + /// visible + public: std::map viewingCollisions; + + /// \brief A map of entity id to thermal camera sensor configuration + /// properties. The elements in the tuple are: + /// + public:std::unordered_map> thermalCameraData; }; ////////////////////////////////////////////////// @@ -242,6 +342,105 @@ rendering::ScenePtr RenderUtil::Scene() const return this->dataPtr->scene; } +////////////////////////////////////////////////// +void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + std::lock_guard lock(this->dataPtr->updateMutex); + + // particle emitters commands + _ecm.Each( + [&](const Entity &_entity, + const components::ParticleEmitterCmd *_emitterCmd) -> bool + { + // store emitter properties and update them in rendering thread + this->dataPtr->newParticleEmittersCmds[_entity] = + _emitterCmd->Data(); + + // update pose comp here + if (_emitterCmd->Data().has_pose()) + { + auto poseComp = _ecm.Component(_entity); + if (poseComp) + poseComp->Data() = msgs::Convert(_emitterCmd->Data().pose()); + } + _ecm.RemoveComponent(_entity); + + return true; + }); + + // Update lights + auto olderEntitiesLightsCmdToDelete = + std::move(this->dataPtr->entityLightsCmdToDelete); + this->dataPtr->entityLightsCmdToDelete.clear(); + + _ecm.Each( + [&](const Entity &_entity, + const components::LightCmd * _lightCmd) -> bool + { + this->dataPtr->entityLights[_entity] = _lightCmd->Data(); + this->dataPtr->entityLightsCmdToDelete.push_back(_entity); + + auto lightComp = _ecm.Component(_entity); + if (lightComp) + { + sdf::Light sdfLight = convert(_lightCmd->Data()); + auto state = lightComp->SetData(sdfLight, + this->dataPtr->lightEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::Light::typeId, state); + } + return true; + }); + + for (const auto entity : olderEntitiesLightsCmdToDelete) + { + _ecm.RemoveComponent(entity); + } + + // Update thermal cameras + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *)->bool + { + // set properties from thermal sensor plugin + // Set defaults to invaid values so we know they have not been set. + // set UpdateECM(). We check for valid values first before setting + // these thermal camera properties.. + double resolution = 0.0; + components::TemperatureRangeInfo range; + range.min = std::numeric_limits::max(); + range.max = 0; + + // resolution + auto resolutionComp = + _ecm.Component(_entity); + if (resolutionComp != nullptr) + { + resolution = resolutionComp->Data(); + _ecm.RemoveComponent( + _entity); + } + + // min / max temp + auto tempRangeComp = + _ecm.Component(_entity); + if (tempRangeComp != nullptr) + { + range = tempRangeComp->Data(); + _ecm.RemoveComponent(_entity); + } + + if (resolutionComp || tempRangeComp) + { + this->dataPtr->thermalCameraData[_entity] = + std::make_tuple(resolution, range); + } + return true; + }); +} + ////////////////////////////////////////////////// void RenderUtil::UpdateFromECM(const UpdateInfo &_info, const EntityComponentManager &_ecm) @@ -254,6 +453,42 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); + this->dataPtr->FindCollisionLinks(_ecm); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) +{ + if (this->newCollisions.empty()) + return; + + for (const auto &entity : this->newCollisions) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId})) + { + links = _ecm.EntitiesByComponents(components::ParentEntity(entity), + components::Link()); + } + else if (_ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links.push_back(entity); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing collision must be a model or link" + << std::endl; + continue; + } + + this->newCollisionLinks.insert(this->newCollisionLinks.end(), + links.begin(), + links.end()); + } + this->newCollisions.clear(); } ////////////////////////////////////////////////// @@ -288,12 +523,18 @@ void RenderUtil::Update() auto newVisuals = std::move(this->dataPtr->newVisuals); auto newActors = std::move(this->dataPtr->newActors); auto newLights = std::move(this->dataPtr->newLights); + auto newParticleEmitters = std::move(this->dataPtr->newParticleEmitters); + auto newParticleEmittersCmds = + std::move(this->dataPtr->newParticleEmittersCmds); auto removeEntities = std::move(this->dataPtr->removeEntities); auto entityPoses = std::move(this->dataPtr->entityPoses); + auto entityLights = std::move(this->dataPtr->entityLights); auto trajectoryPoses = std::move(this->dataPtr->trajectoryPoses); auto actorTransforms = std::move(this->dataPtr->actorTransforms); auto actorAnimationData = std::move(this->dataPtr->actorAnimationData); auto entityTemp = std::move(this->dataPtr->entityTemp); + auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); + auto thermalCameraData = std::move(this->dataPtr->thermalCameraData); this->dataPtr->newScenes.clear(); this->dataPtr->newModels.clear(); @@ -301,12 +542,17 @@ void RenderUtil::Update() this->dataPtr->newVisuals.clear(); this->dataPtr->newActors.clear(); this->dataPtr->newLights.clear(); + this->dataPtr->newParticleEmitters.clear(); + this->dataPtr->newParticleEmittersCmds.clear(); this->dataPtr->removeEntities.clear(); this->dataPtr->entityPoses.clear(); + this->dataPtr->entityLights.clear(); this->dataPtr->trajectoryPoses.clear(); this->dataPtr->actorTransforms.clear(); this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->newCollisionLinks.clear(); + this->dataPtr->thermalCameraData.clear(); this->dataPtr->markerManager.Update(); @@ -420,6 +666,18 @@ void RenderUtil::Update() } } + for (const auto &emitter : newParticleEmitters) + { + this->dataPtr->sceneManager.CreateParticleEmitter( + std::get<0>(emitter), std::get<1>(emitter), std::get<2>(emitter)); + } + + for (const auto &emitterCmd : newParticleEmittersCmds) + { + this->dataPtr->sceneManager.UpdateParticleEmitter( + emitterCmd.first, emitterCmd.second); + } + if (this->dataPtr->enableSensors && this->dataPtr->createSensorCb) { for (const auto &sensor : newSensors) @@ -453,6 +711,95 @@ void RenderUtil::Update() } } + // update lights + { + IGN_PROFILE("RenderUtil::Update Lights"); + for (const auto &light : entityLights) { + auto node = this->dataPtr->sceneManager.NodeById(light.first); + if (!node) + continue; + auto l = std::dynamic_pointer_cast(node); + if (l) + { + if (light.second.has_diffuse()) + { + if (l->DiffuseColor() != msgs::Convert(light.second.diffuse())) + l->SetDiffuseColor(msgs::Convert(light.second.diffuse())); + } + if (light.second.has_specular()) + { + if (l->SpecularColor() != msgs::Convert(light.second.specular())) + { + l->SetSpecularColor(msgs::Convert(light.second.specular())); + } + } + if (!ignition::math::equal( + l->AttenuationRange(), + static_cast(light.second.range()))) + { + l->SetAttenuationRange(light.second.range()); + } + if (!ignition::math::equal( + l->AttenuationLinear(), + static_cast(light.second.attenuation_linear()))) + { + l->SetAttenuationLinear(light.second.attenuation_linear()); + } + if (!ignition::math::equal( + l->AttenuationConstant(), + static_cast(light.second.attenuation_constant()))) + { + l->SetAttenuationConstant(light.second.attenuation_constant()); + } + if (!ignition::math::equal( + l->AttenuationQuadratic(), + static_cast(light.second.attenuation_quadratic()))) + { + l->SetAttenuationQuadratic(light.second.attenuation_quadratic()); + } + if (l->CastShadows() != light.second.cast_shadows()) + l->SetCastShadows(light.second.cast_shadows()); + auto lDirectional = + std::dynamic_pointer_cast(node); + if (lDirectional) + { + if (light.second.has_direction()) + { + if (lDirectional->Direction() != + msgs::Convert(light.second.direction())) + { + lDirectional->SetDirection( + msgs::Convert(light.second.direction())); + } + } + } + auto lSpotLight = + std::dynamic_pointer_cast(node); + if (lSpotLight) + { + if (light.second.has_direction()) + { + if (lSpotLight->Direction() != + msgs::Convert(light.second.direction())) + { + lSpotLight->SetDirection( + msgs::Convert(light.second.direction())); + } + } + if (lSpotLight->InnerAngle() != light.second.spot_inner_angle()) + lSpotLight->SetInnerAngle(light.second.spot_inner_angle()); + if (lSpotLight->OuterAngle() != light.second.spot_outer_angle()) + lSpotLight->SetOuterAngle(light.second.spot_outer_angle()); + if (!ignition::math::equal( + lSpotLight->Falloff(), + static_cast(light.second.spot_falloff()))) + { + lSpotLight->SetFalloff(light.second.spot_falloff()); + } + } + } + } + } // update entities' pose { IGN_PROFILE("RenderUtil::Update Poses"); @@ -622,7 +969,7 @@ void RenderUtil::Update() } // set visual temperature - for (auto &temp : entityTemp) + for (const auto &temp : entityTemp) { auto node = this->dataPtr->sceneManager.NodeById(temp.first); if (!node) @@ -633,7 +980,90 @@ void RenderUtil::Update() if (!visual) continue; - visual->SetUserData("temperature", temp.second); + const auto &heatSignature = std::get<2>(temp.second); + if (heatSignature.empty()) + visual->SetUserData("temperature", std::get<0>(temp.second)); + else + { + visual->SetUserData("minTemp", std::get<0>(temp.second)); + visual->SetUserData("maxTemp", std::get<1>(temp.second)); + visual->SetUserData("temperature", heatSignature); + } + } + + // create new collision visuals + { + for (const auto &link : newCollisionLinks) + { + std::vector colEntities = + this->dataPtr->linkToCollisionEntities[link]; + + for (const auto &colEntity : colEntities) + { + if (!this->dataPtr->sceneManager.HasEntity(colEntity)) + { + auto vis = this->dataPtr->sceneManager.CreateCollision(colEntity, + this->dataPtr->entityCollisions[colEntity], link); + this->dataPtr->viewingCollisions[colEntity] = true; + + // add geometry material to originalEmissive map + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + auto geom = vis->GeometryByIndex(g); + + // Geometry material + auto geomMat = geom->Material(); + if (nullptr == geomMat) + continue; + + if (this->dataPtr->originalEmissive.find(geom->Name()) == + this->dataPtr->originalEmissive.end()) + { + this->dataPtr->originalEmissive[geom->Name()] = + geomMat->Emissive(); + } + } + } + } + } + } + + // update thermal camera + for (const auto &thermal : this->dataPtr->thermalCameraData) + { + Entity id = thermal.first; + rendering::ThermalCameraPtr camera = + std::dynamic_pointer_cast( + this->dataPtr->sceneManager.NodeById(id)); + if (camera) + { + double resolution = std::get<0>(thermal.second); + + if (resolution > 0.0) + { + camera->SetLinearResolution(resolution); + } + else + { + ignwarn << "Unable to set thermal camera temperature linear resolution." + << " Value must be greater than 0. Using the default value: " + << camera->LinearResolution() << ". " << std::endl; + } + double minTemp = std::get<1>(thermal.second).min.Kelvin(); + double maxTemp = std::get<1>(thermal.second).max.Kelvin(); + if (maxTemp >= minTemp) + { + camera->SetMinTemperature(minTemp); + camera->SetMaxTemperature(maxTemp); + } + else + { + ignwarn << "Unable to set thermal camera temperature range." + << "Max temperature must be greater or equal to min. " + << "Using the default values : [" << camera->MinTemperature() + << ", " << camera->MaxTemperature() << "]." << std::endl; + } + } } } @@ -715,6 +1145,8 @@ void RenderUtilPrivate::CreateRenderingEntities( link.SetRawPose(_pose->Data()); this->newLinks.push_back( std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; }); @@ -750,13 +1182,35 @@ void RenderUtilPrivate::CreateRenderingEntities( visual.SetMaterial(material->Data()); } - // todo(anyone) make visual updates more generic without using extra - // variables like entityTemp just for storing one specific visual - // param? - auto temp = _ecm.Component(_entity); - if (temp) + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) + { + visual.SetLaserRetro(laserRetro->Data()); + } + + if (auto temp = _ecm.Component(_entity)) { - this->entityTemp[_entity] = temp->Data().Kelvin(); + // get the uniform temperature for the entity + this->entityTemp[_entity] = + std::make_tuple( + temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) + { + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); + } } this->newVisuals.push_back( @@ -786,6 +1240,34 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); + // collisions + _ecm.Each( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); + + // particle emitters + _ecm.Each( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent) -> bool + { + this->newParticleEmitters.push_back( + std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -891,6 +1373,8 @@ void RenderUtilPrivate::CreateRenderingEntities( link.SetRawPose(_pose->Data()); this->newLinks.push_back( std::make_tuple(_entity, link, _parent->Data())); + // used for collsions + this->modelToLinkEntities[_parent->Data()].push_back(_entity); return true; }); @@ -926,6 +1410,37 @@ void RenderUtilPrivate::CreateRenderingEntities( visual.SetMaterial(material->Data()); } + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) + { + visual.SetLaserRetro(laserRetro->Data()); + } + + if (auto temp = _ecm.Component(_entity)) + { + // get the uniform temperature for the entity + this->entityTemp[_entity] = + std::make_tuple( + temp->Data().Kelvin(), 0.0, ""); + } + else + { + // entity doesn't have a uniform temperature. Check if it has + // a heat signature with an associated temperature range + auto heatSignature = + _ecm.Component(_entity); + auto tempRange = + _ecm.Component(_entity); + if (heatSignature && tempRange) + { + this->entityTemp[_entity] = + std::make_tuple( + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); + } + } + this->newVisuals.push_back( std::make_tuple(_entity, visual, _parent->Data())); return true; @@ -953,6 +1468,34 @@ void RenderUtilPrivate::CreateRenderingEntities( return true; }); + // collisions + _ecm.EachNew( + [&](const Entity &_entity, + const components::Collision *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CollisionElement *_collElement, + const components::ParentEntity *_parent) -> bool + { + this->entityCollisions[_entity] = _collElement->Data(); + this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + return true; + }); + + // particle emitters + _ecm.EachNew( + [&](const Entity &_entity, + const components::ParticleEmitter *_emitter, + const components::ParentEntity *_parent) -> bool + { + this->newParticleEmitters.push_back( + std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -1096,7 +1639,7 @@ void RenderUtilPrivate::UpdateRenderingEntities( return true; }); - // lights + // update lights _ecm.Each( [&](const Entity &_entity, const components::Light *, @@ -1166,6 +1709,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Model *)->bool { this->removeEntities[_entity] = _info.iterations; + this->modelToLinkEntities.erase(_entity); return true; }); @@ -1173,6 +1717,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Link *)->bool { this->removeEntities[_entity] = _info.iterations; + this->linkToCollisionEntities.erase(_entity); return true; }); @@ -1195,6 +1740,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // particle emitters + _ecm.EachRemoved( + [&](const Entity &_entity, const components::ParticleEmitter *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // cameras _ecm.EachRemoved( [&](const Entity &_entity, const components::Camera *)->bool @@ -1234,6 +1787,16 @@ void RenderUtilPrivate::RemoveRenderingEntities( this->removeEntities[_entity] = _info.iterations; return true; }); + + // collisions + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Collision *)->bool + { + this->removeEntities[_entity] = _info.iterations; + this->viewingCollisions.erase(_entity); + this->entityCollisions.erase(_entity); + return true; + }); } ///////////////////////////////////////////////// @@ -1511,3 +2074,65 @@ void RenderUtilPrivate::LowlightNode(const rendering::NodePtr &_node) visParent->SetVisible(false); } } + +///////////////////////////////////////////////// +void RenderUtil::ViewCollisions(const Entity &_entity) +{ + std::vector colEntities; + if (this->dataPtr->linkToCollisionEntities.find(_entity) != + this->dataPtr->linkToCollisionEntities.end()) + { + colEntities = this->dataPtr->linkToCollisionEntities[_entity]; + } + else if (this->dataPtr->modelToLinkEntities.find(_entity) != + this->dataPtr->modelToLinkEntities.end()) + { + std::vector links = this->dataPtr->modelToLinkEntities[_entity]; + for (const auto &link : links) + colEntities.insert(colEntities.end(), + this->dataPtr->linkToCollisionEntities[link].begin(), + this->dataPtr->linkToCollisionEntities[link].end()); + } + + // create and/or toggle collision visuals + + bool showCol, showColInit = false; + // first loop looks for new collisions + for (const auto &colEntity : colEntities) + { + if (this->dataPtr->viewingCollisions.find(colEntity) == + this->dataPtr->viewingCollisions.end()) + { + this->dataPtr->newCollisions.push_back(_entity); + showColInit = showCol = true; + } + } + + // second loop toggles already created collisions + for (const auto &colEntity : colEntities) + { + if (this->dataPtr->viewingCollisions.find(colEntity) == + this->dataPtr->viewingCollisions.end()) + continue; + + // when viewing multiple collisions (e.g. _entity is a model), + // boolean for view collisions is based on first colEntity in list + if (!showColInit) + { + showCol = !this->dataPtr->viewingCollisions[colEntity]; + showColInit = true; + } + + rendering::VisualPtr colVisual = + this->dataPtr->sceneManager.VisualById(colEntity); + if (colVisual == nullptr) + { + ignerr << "Could not find collision visual for entity [" << colEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingCollisions[colEntity] = showCol; + colVisual->SetVisible(showCol); + } +} diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 84eec0b160..6c0d86aa52 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -19,6 +19,7 @@ #include #include +#include #include #include #include @@ -31,9 +32,11 @@ #include #include #include +#include #include #include -#include + +#include #include #include @@ -41,9 +44,11 @@ #include #include #include +#include #include #include +#include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/Util.hh" #include "ignition/gazebo/rendering/SceneManager.hh" @@ -80,6 +85,10 @@ class ignition::gazebo::SceneManagerPrivate /// \brief Map of light entity in Gazebo to light pointers. public: std::map lights; + /// \brief Map of particle emitter entity in Gazebo to particle emitter + /// rendering pointers. + public: std::map particleEmitters; + /// \brief Map of sensor entity in Gazebo to sensor pointers. public: std::map sensors; @@ -264,6 +273,11 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, visualVis->SetUserData("pause-update", static_cast(0)); visualVis->SetLocalPose(_visual.RawPose()); + if (_visual.HasLaserRetro()) + { + visualVis->SetUserData("laser_retro", _visual.LaserRetro()); + } + math::Vector3d scale = math::Vector3d::One; math::Pose3d localPose; rendering::GeometryPtr geom = @@ -274,17 +288,19 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, /// localPose is currently used to handle the normal vector in plane visuals /// In general, this can be used to store any local transforms between the /// parent Visual and geometry. - rendering::VisualPtr geomVis; if (localPose != math::Pose3d::Zero) { - geomVis = this->dataPtr->scene->CreateVisual(name + "_geom"); - geomVis->SetUserData("gazebo-entity", static_cast(_id)); - geomVis->SetUserData("pause-update", static_cast(0)); - geomVis->SetLocalPose(_visual.RawPose() * localPose); - visualVis = geomVis; + rendering::VisualPtr geomVis = + this->dataPtr->scene->CreateVisual(name + "_geom"); + geomVis->AddGeometry(geom); + geomVis->SetLocalPose(localPose); + visualVis->AddChild(geomVis); + } + else + { + visualVis->AddGeometry(geom); } - visualVis->AddGeometry(geom); visualVis->SetLocalScale(scale); // set material @@ -365,6 +381,36 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, return visualVis; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::VisualById(Entity _id) +{ + if (this->dataPtr->visuals.find(_id) == this->dataPtr->visuals.end()) + { + ignerr << "Could not find visual for entity: " << _id << std::endl; + return nullptr; + } + return this->dataPtr->visuals[_id]; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateCollision(Entity _id, + const sdf::Collision &_collision, Entity _parentId) +{ + sdf::Material material; + material.SetAmbient(math::Color(1, 0.5088, 0.0468, 0.7)); + material.SetDiffuse(math::Color(1, 0.5088, 0.0468, 0.7)); + + sdf::Visual visual; + visual.SetGeom(*_collision.Geom()); + visual.SetMaterial(material); + visual.SetCastShadows(false); + + visual.SetRawPose(_collision.RawPose()); + visual.SetName(_collision.Name()); + + rendering::VisualPtr collisionVis = CreateVisual(_id, visual, _parentId); + return collisionVis; +} ///////////////////////////////////////////////// rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, math::Vector3d &_scale, math::Pose3d &_localPose) @@ -1081,6 +1127,148 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, return light; } +///////////////////////////////////////////////// +rendering::ParticleEmitterPtr SceneManager::CreateParticleEmitter( + Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::ParticleEmitterPtr(); + + if (this->dataPtr->particleEmitters.find(_id) != + this->dataPtr->particleEmitters.end()) + { + ignerr << "Particle emitter with Id: [" << _id << "] already exists in the " + <<" scene" << std::endl; + return rendering::ParticleEmitterPtr(); + } + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + // It is possible to get here if the model entity is created then + // removed in between render updates. + return rendering::ParticleEmitterPtr(); + } + parent = it->second; + } + + // Name. + std::string name = _emitter.name().empty() ? std::to_string(_id) : + _emitter.name(); + if (parent) + name = parent->Name() + "::" + name; + + rendering::ParticleEmitterPtr emitter; + emitter = this->dataPtr->scene->CreateParticleEmitter(name); + + this->dataPtr->particleEmitters[_id] = emitter; + + if (parent) + parent->AddChild(emitter); + + this->UpdateParticleEmitter(_id, _emitter); + + return emitter; +} + +///////////////////////////////////////////////// +rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id, + const msgs::ParticleEmitter &_emitter) +{ + if (!this->dataPtr->scene) + return rendering::ParticleEmitterPtr(); + + // Sanity check: Make sure that the id exists. + auto emitterIt = this->dataPtr->particleEmitters.find(_id); + if (emitterIt == this->dataPtr->particleEmitters.end()) + { + ignerr << "Particle emitter with Id: [" << _id << "] not found in the " + << "scene" << std::endl; + return rendering::ParticleEmitterPtr(); + } + auto emitter = emitterIt->second; + + // Type. + switch (_emitter.type()) + { + case ignition::msgs::ParticleEmitter_EmitterType_BOX: + { + emitter->SetType(ignition::rendering::EmitterType::EM_BOX); + break; + } + case ignition::msgs::ParticleEmitter_EmitterType_CYLINDER: + { + emitter->SetType(ignition::rendering::EmitterType::EM_CYLINDER); + break; + } + case ignition::msgs::ParticleEmitter_EmitterType_ELLIPSOID: + { + emitter->SetType(ignition::rendering::EmitterType::EM_ELLIPSOID); + break; + } + default: + { + emitter->SetType(ignition::rendering::EmitterType::EM_POINT); + } + } + + // Emitter size. + if (_emitter.has_size()) + emitter->SetEmitterSize(ignition::msgs::Convert(_emitter.size())); + + // Rate. + emitter->SetRate(_emitter.rate()); + + // Duration. + emitter->SetDuration(_emitter.duration()); + + // Emitting. + emitter->SetEmitting(_emitter.emitting()); + + // Particle size. + emitter->SetParticleSize( + ignition::msgs::Convert(_emitter.particle_size())); + + // Lifetime. + emitter->SetLifetime(_emitter.lifetime()); + + // Material. + if (_emitter.has_material()) + { + ignition::rendering::MaterialPtr material = + this->LoadMaterial(convert(_emitter.material())); + emitter->SetMaterial(material); + } + + // Velocity range. + emitter->SetVelocityRange(_emitter.min_velocity(), _emitter.max_velocity()); + + // Color range image. + if (!_emitter.color_range_image().empty()) + { + emitter->SetColorRangeImage(_emitter.color_range_image()); + } + // Color range. + else if (_emitter.has_color_start() && _emitter.has_color_end()) + { + emitter->SetColorRange( + ignition::msgs::Convert(_emitter.color_start()), + ignition::msgs::Convert(_emitter.color_end())); + } + + // Scale rate. + emitter->SetScaleRate(_emitter.scale_rate()); + + // pose + if (_emitter.has_pose()) + emitter->SetLocalPose(msgs::Convert(_emitter.pose())); + + return emitter; +} + ///////////////////////////////////////////////// bool SceneManager::AddSensor(Entity _gazeboId, const std::string &_sensorName, Entity _parentGazeboId) @@ -1131,6 +1319,8 @@ bool SceneManager::HasEntity(Entity _id) const return this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end() || this->dataPtr->actors.find(_id) != this->dataPtr->actors.end() || this->dataPtr->lights.find(_id) != this->dataPtr->lights.end() || + this->dataPtr->particleEmitters.find(_id) != + this->dataPtr->particleEmitters.end() || this->dataPtr->sensors.find(_id) != this->dataPtr->sensors.end(); } @@ -1142,22 +1332,22 @@ rendering::NodePtr SceneManager::NodeById(Entity _id) const { return vIt->second; } - else + auto lIt = this->dataPtr->lights.find(_id); + if (lIt != this->dataPtr->lights.end()) { - auto lIt = this->dataPtr->lights.find(_id); - if (lIt != this->dataPtr->lights.end()) - { - return lIt->second; - } - else - { - auto sIt = this->dataPtr->sensors.find(_id); - if (sIt != this->dataPtr->sensors.end()) - { - return sIt->second; - } - } + return lIt->second; + } + auto sIt = this->dataPtr->sensors.find(_id); + if (sIt != this->dataPtr->sensors.end()) + { + return sIt->second; } + auto pIt = this->dataPtr->particleEmitters.find(_id); + if (pIt != this->dataPtr->particleEmitters.end()) + { + return pIt->second; + } + return rendering::NodePtr(); } @@ -1379,6 +1569,16 @@ void SceneManager::RemoveEntity(Entity _id) } } + { + auto it = this->dataPtr->particleEmitters.find(_id); + if (it != this->dataPtr->particleEmitters.end()) + { + this->dataPtr->scene->DestroyVisual(it->second); + this->dataPtr->particleEmitters.erase(it); + return; + } + } + { auto it = this->dataPtr->sensors.find(_id); if (it != this->dataPtr->sensors.end()) diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index cf7c9d9672..9105d9b285 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -91,6 +91,7 @@ add_subdirectory(apply_joint_force) add_subdirectory(battery_plugin) add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) +add_subdirectory(collada_world_exporter) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) @@ -100,6 +101,7 @@ add_subdirectory(imu) add_subdirectory(joint_controller) add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) +add_subdirectory(joint_trajectory_controller) add_subdirectory(kinetic_energy_monitor) add_subdirectory(lift_drag) add_subdirectory(log) @@ -110,6 +112,7 @@ add_subdirectory(magnetometer) add_subdirectory(multicopter_motor_model) add_subdirectory(multicopter_control) add_subdirectory(optical_tactile_plugin) +add_subdirectory(particle_emitter) add_subdirectory(performer_detector) add_subdirectory(physics) add_subdirectory(pose_publisher) diff --git a/src/systems/collada_world_exporter/CMakeLists.txt b/src/systems/collada_world_exporter/CMakeLists.txt new file mode 100644 index 0000000000..a4bae60924 --- /dev/null +++ b/src/systems/collada_world_exporter/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(collada-world-exporter + SOURCES + ColladaWorldExporter.cc +) diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.cc b/src/systems/collada_world_exporter/ColladaWorldExporter.cc new file mode 100644 index 0000000000..0ab619f899 --- /dev/null +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.cc @@ -0,0 +1,266 @@ +/* + * Copyright (C) 2020 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 +#include +#include + +#include +#include +#include +#include + +#include + +#include + +#include "ColladaWorldExporter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + + +class ignition::gazebo::systems::ColladaWorldExporterPrivate +{ + // Default constructor + public: ColladaWorldExporterPrivate() = default; + + /// \brief Has the world already been exported?. + private: bool exported{false}; + + /// \brief Exports the world to a mesh. + /// \param[_ecm] _ecm Mutable reference to the EntityComponentManager. + public: void Export(const EntityComponentManager &_ecm) + { + if (this->exported) return; + + common::Mesh worldMesh; + std::vector subMeshMatrix; + + _ecm.Each( + [&](const Entity /*& _entity*/, + const components::World *, + const components::Name * _name)->bool + { + worldMesh.SetName(_name->Data()); + return true; + }); + + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Geometry *_geom, + const components::Transparency *_transparency)->bool + { + std::string name = _name->Data().empty() ? std::to_string(_entity) : + _name->Data(); + + math::Pose3d worldPose = gazebo::worldPose(_entity, _ecm); + + common::MaterialPtr mat = std::make_shared(); + auto material = _ecm.Component(_entity); + if (material != nullptr) + { + mat->SetDiffuse(material->Data().Diffuse()); + mat->SetAmbient(material->Data().Ambient()); + mat->SetEmissive(material->Data().Emissive()); + mat->SetSpecular(material->Data().Specular()); + } + mat->SetTransparency(_transparency->Data()); + + const ignition::common::Mesh *mesh; + std::weak_ptr subm; + math::Vector3d scale; + math::Matrix4d matrix(worldPose); + ignition::common::MeshManager *meshManager = + ignition::common::MeshManager::Instance(); + + auto addSubmeshFunc = [&](int i) { + subm = worldMesh.AddSubMesh( + *mesh->SubMeshByIndex(0).lock().get()); + subm.lock()->SetMaterialIndex(i); + subm.lock()->Scale(scale); + subMeshMatrix.push_back(matrix); + }; + + if (_geom->Data().Type() == sdf::GeometryType::BOX) + { + if (meshManager->HasMesh("unit_box")) + { + mesh = meshManager->MeshByName("unit_box"); + scale = _geom->Data().BoxShape()->Size(); + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::CYLINDER) + { + if (meshManager->HasMesh("unit_cylinder")) + { + mesh = meshManager->MeshByName("unit_cylinder"); + scale.X() = _geom->Data().CylinderShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _geom->Data().CylinderShape()->Length(); + + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::PLANE) + { + if (meshManager->HasMesh("unit_plane")) + { + // Create a rotation for the plane mesh to account + // for the normal vector. + mesh = meshManager->MeshByName("unit_plane"); + + scale.X() = _geom->Data().PlaneShape()->Size().X(); + scale.Y() = _geom->Data().PlaneShape()->Size().Y(); + + // // The rotation is the angle between the +z(0,0,1) vector and the + // // normal, which are both expressed in the local (Visual) frame. + math::Vector3d normal = _geom->Data().PlaneShape()->Normal(); + math::Quaterniond normalRot; + normalRot.From2Axes(math::Vector3d::UnitZ, normal.Normalized()); + worldPose.Rot() = worldPose.Rot() * normalRot; + + matrix = math::Matrix4d(worldPose); + + int i = worldMesh.AddMaterial(mat); + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::SPHERE) + { + if (meshManager->HasMesh("unit_sphere")) + { + mesh = meshManager->MeshByName("unit_sphere"); + + scale.X() = _geom->Data().SphereShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = scale.X(); + + int i = worldMesh.AddMaterial(mat); + + addSubmeshFunc(i); + } + } + else if (_geom->Data().Type() == sdf::GeometryType::MESH) + { + auto fullPath = asFullPath(_geom->Data().MeshShape()->Uri(), + _geom->Data().MeshShape()->FilePath()); + + if (fullPath.empty()) + { + ignerr << "Mesh geometry missing uri" << std::endl; + return true; + } + mesh = meshManager->Load(fullPath); + + if (!mesh) { + ignerr << "mesh not found!" << std::endl; + return true; + } + + for (unsigned int k = 0; k < mesh->SubMeshCount(); k++) + { + auto subMeshLock = mesh->SubMeshByIndex(k).lock(); + int j = subMeshLock->MaterialIndex(); + + int i = 0; + if (j != -1) + { + i = worldMesh.IndexOfMaterial(mesh->MaterialByIndex(j).get()); + if (i < 0) + { + i = worldMesh.AddMaterial(mesh->MaterialByIndex(j)); + } + } + else + { + i = worldMesh.AddMaterial(mat); + } + + scale = _geom->Data().MeshShape()->Scale(); + + addSubmeshFunc(i); + } + } + else + { + ignwarn << "Unsupported geometry type" << std::endl; + } + + return true; + }); + + common::ColladaExporter exporter; + exporter.Export(&worldMesh, "./" + worldMesh.Name(), true, + subMeshMatrix); + ignmsg << "The world has been exported into the " + << "./" + worldMesh.Name() << " directory." << std::endl; + this->exported = true; + } +}; + +///////////////////////////////////////////////// +ColladaWorldExporter::ColladaWorldExporter() : System(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ColladaWorldExporter::~ColladaWorldExporter() = default; + +///////////////////////////////////////////////// +void ColladaWorldExporter::PostUpdate(const UpdateInfo & /*_info*/, + const EntityComponentManager &_ecm) +{ + this->dataPtr->Export(_ecm); +} + +IGNITION_ADD_PLUGIN(ColladaWorldExporter, + System, + ColladaWorldExporter::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ColladaWorldExporter, + "ignition::gazebo::systems::ColladaWorldExporter") diff --git a/src/systems/collada_world_exporter/ColladaWorldExporter.hh b/src/systems/collada_world_exporter/ColladaWorldExporter.hh new file mode 100644 index 0000000000..731c9ec2ee --- /dev/null +++ b/src/systems/collada_world_exporter/ColladaWorldExporter.hh @@ -0,0 +1,59 @@ +/* + * Copyright (C) 2020 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_COLLADAWORLDEXPORTER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_COLLADAWORLDEXPORTER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ColladaWorldExporterPrivate; + + /// \brief A plugin that exports a world to a mesh. + /// When loaded the plugin will dump a mesh containing all the models in + /// the world to the current directory. + class ColladaWorldExporter: + public System, + public ISystemPostUpdate + { + /// \brief Constructor + public: ColladaWorldExporter(); + + /// \brief Destructor + public: ~ColladaWorldExporter() final; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/diff_drive/DiffDrive.cc b/src/systems/diff_drive/DiffDrive.cc index 917d65bf15..29a095a79f 100644 --- a/src/systems/diff_drive/DiffDrive.cc +++ b/src/systems/diff_drive/DiffDrive.cc @@ -121,6 +121,9 @@ class ignition::gazebo::systems::DiffDrivePrivate /// \brief Diff drive odometry message publisher. public: transport::Node::Publisher odomPub; + /// \brief Diff drive tf message publisher. + public: transport::Node::Publisher tfPub; + /// \brief Linear velocity limiter. public: std::unique_ptr limiterLin; @@ -283,6 +286,13 @@ void DiffDrive::Configure(const Entity &_entity, this->dataPtr->odomPub = this->dataPtr->node.Advertise( odomTopic); + std::string tfTopic{"/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"}; + if (_sdf->HasElement("tf_topic")) + tfTopic = _sdf->Get("tf_topic"); + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + if (_sdf->HasElement("frame_id")) this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); @@ -506,9 +516,17 @@ void DiffDrivePrivate::UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, childFrame->add_value(this->sdfChildFrameId); } + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + ignition::msgs::Pose *tfMsgPose = nullptr; + tfMsgPose = tfMsg.add_pose(); + tfMsgPose->mutable_header()->CopyFrom(*msg.mutable_header()); + tfMsgPose->mutable_position()->CopyFrom(msg.mutable_pose()->position()); + tfMsgPose->mutable_orientation()->CopyFrom(msg.mutable_pose()->orientation()); - // Publish the message + // Publish the messages this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); } ////////////////////////////////////////////////// diff --git a/src/systems/diff_drive/DiffDrive.hh b/src/systems/diff_drive/DiffDrive.hh index f27e34812e..35002495d5 100644 --- a/src/systems/diff_drive/DiffDrive.hh +++ b/src/systems/diff_drive/DiffDrive.hh @@ -61,6 +61,22 @@ namespace systems /// ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is /// `/model/{name_of_model}/odometry`. + /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element if optional, + /// and the default value is `/model/{name_of_model}/tf`. + /// + /// ``: Custom `frame_id` field that this system will use as the + /// origin of the odometry transform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, and the + /// default value is `{name_of_model}/odom`. + /// + /// ``: Custom `child_frame_id` that this system will use as + /// the target of the odometry trasnform in both the `` + /// `ignition.msgs.Pose_V` message and the `` + /// `ignition.msgs.Odometry` message. This element if optional, + /// and the default value is `{name_of_model}/{name_of_link}`. class DiffDrive : public System, public ISystemConfigure, diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index dd2f645f1d..dce17b55c3 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -140,6 +140,19 @@ void JointController::Configure(const Entity &_entity, << "]" << std::endl; return; } + if (_sdf->HasElement("topic")) + { + topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + + if (topic.empty()) + { + ignerr << "Failed to create topic [" << _sdf->Get("topic") + << "]" << " for joint [" << this->dataPtr->jointName + << "]" << std::endl; + return; + } + } this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, this->dataPtr.get()); diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 5eaefaf45e..5c14e0f082 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -156,6 +156,19 @@ void JointPositionController::Configure(const Entity &_entity, << "]" << std::endl; return; } + if (_sdf->HasElement("topic")) + { + topic = transport::TopicUtils::AsValidTopic( + _sdf->Get("topic")); + + if (topic.empty()) + { + ignerr << "Failed to create topic [" << _sdf->Get("topic") + << "]" << " for joint [" << this->dataPtr->jointName + << "]" << std::endl; + return; + } + } this->dataPtr->node.Subscribe( topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); diff --git a/src/systems/joint_trajectory_controller/CMakeLists.txt b/src/systems/joint_trajectory_controller/CMakeLists.txt new file mode 100644 index 0000000000..c4150b372e --- /dev/null +++ b/src/systems/joint_trajectory_controller/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(joint-trajectory-controller + SOURCES + JointTrajectoryController.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/joint_trajectory_controller/JointTrajectoryController.cc b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc new file mode 100644 index 0000000000..d7cb345e60 --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.cc @@ -0,0 +1,1067 @@ +/* + * 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 + +#include +#include +#include + +#include "JointTrajectoryController.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Helper class that contains all parameters required to create and +/// configure an instance of ActuatedJoint +class JointParameters +{ + /// \brief Parse all parameters required for creation of ActuatedJoint and + /// return them in a map + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _ecm Ignition Entity Component Manager + /// \param[in] _enabledJoints List of joint entities that are enabled and + /// need to be created + /// \return Map of parameters for each joint, the first entry of pair + /// indicates the joint name + public: static std::map ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints); + + /// \brief Parse all values of a single parameter that is specified multiple + /// times in SDF + /// \param[in] _sdf SDF reference used to obtain the parameters + /// \param[in] _parameterName Name of the repeated parameter to parse all + /// values for + /// \return Ordered list of all values for a given repeated parameter + public: template static std::vector Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName); + + /// \brief Return value from `_vec` at `_index`, or `_alternative_value` if + /// `_vec` is not large enough + /// \param[in] _vec Vector that contains desired value of type T + /// \param[in] _index Index at which the value is stored + /// \param[in] _alternative_value Alternative or default value of type T + /// \return Value from `_vec` at `_index`, or `_alternative_value` if `_vec` + /// is not large enough + public: template static T NthElementOr( + const std::vector &_vec, + const size_t &_index, + const T &_alternative_value); + + /// \brief Initial position of the joint + public: double initialPosition; + /// \brief Default value for initial position of the joint + public: static constexpr double initialPositionDefault = 0.0; + + /// \brief Parameters required for creation of new PID controller + public: struct PID + { + /// \brief Proportional gain + double pGain; + /// \brief Default value for proportional gain + static constexpr double pGainDefault = 0.0; + + /// \brief Integral gain + double iGain; + /// \brief Default value for integral gain + static constexpr double iGainDefault = 0.0; + + /// \brief Derivative gain + double dGain; + /// \brief Default value for derivative gain + static constexpr double dGainDefault = 0.0; + + /// \brief Integral lower limit + double iMin; + /// \brief Default value for integral lower limit + static constexpr double iMinDefault = 0.0; + + /// \brief Integral upper limit + double iMax; + /// \brief Default value for integral upper limit + static constexpr double iMaxDefault = -1.0; + + /// \brief Output min value + double cmdMin; + /// \brief Default value for output min value + static constexpr double cmdMinDefault = 0.0; + + /// \brief Output max value + double cmdMax; + /// \brief Default value for output max value + static constexpr double cmdMaxDefault = -1.0; + + /// \brief Output offset + double cmdOffset; + /// \brief Default value for output offset + static constexpr double cmdOffsetDefault = 0.0; + } positionPID, velocityPID; +}; + +/// \brief A single 1-axis joint that is controlled by JointTrajectoryController +/// plugin +class ActuatedJoint +{ + /// \brief Default contructor + public: ActuatedJoint() = default; + + /// \brief Constructor that properly configures the actuated joint + /// \param[in] _entity Entity of the joint + /// \param[in] _params All parameters of the joint required for its + /// configuration + public: ActuatedJoint(const Entity &_entity, + const JointParameters &_params); + + /// \brief Setup components required for control of this joint + /// \param[in,out] _ecm Ignition Entity Component Manager + public: void SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const; + + /// \brief Set target of the joint that the controller will attempt to reach + /// \param[in] _targetPoint Targets of all controlled joint + /// \param[in] _jointIndex Index of the joint, used to determine what index + /// of `_targetPoint` to use + public: void SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex); + + /// \brief Update command force that is applied on the joint + /// \param[in,out] _ecm Ignition Entity Component Manager + /// \param[in] _dt Time difference to update for + public: void Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt); + + /// \brief Reset the target of the joint + public: void ResetTarget(); + + /// \brief Reset the position and velocity PID error on the joint + public: void ResetPIDs(); + + /// \brief Entity of the joint + public: Entity entity; + + /// \brief Target state that the joint controller should reach + public: struct TargetState + { + /// \brief Target position of the joint + double position; + /// \brief Target position of the joint + double velocity; + /// \brief Target acceleration of the joint + /// \attention Acceleration control is NOT implemented at the moment + double acceleration; + /// \brief Target force or torque of the joint + double effort; + } target; + + /// \brief Initial position of the joint + public: double initialPosition; + + /// \brief List of PID controllers used for the control of this actuated joint + public: struct PIDs + { + /// \brief Position PID controller + ignition::math::PID position; + /// \brief Velocity PID controller + ignition::math::PID velocity; + } pids; +}; + +/// \brief Information about trajectory that is followed by +/// JointTrajectoryController plugin +class Trajectory +{ + /// \brief Update index of trajectory points, such that it directs to a point + /// that needs to be currently followed + /// \param[in] _simTime Current simulation time + /// \return True if index of the trajectory point was updated, False otherwise + public: bool UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime); + + /// \brief Determine if the trajectory goal was reached + /// \return True if trajectory goal was reached, False otherwise + public: bool IsGoalReached() const; + + /// \brief Compute progress of the current trajectory + /// \return Fraction of the completed points in range (0.0, 1.0] + public: float ComputeProgress() const; + + /// \brief Reset trajectory internals, i.e. clean list of joint names, points + /// and reset index of the current point + public: void Reset(); + + /// \brief Status of the trajectory + public: enum TrajectoryStatus + { + /// \brief Trajectory is new and needs to be configure on the next update + /// loop + New, + /// \brief Trajectory is currently being followed + Active, + /// \brief Trajectory goal is reached + Reached, + } status; + + /// \brief Start time of trajectory + public: std::chrono::steady_clock::duration startTime; + + /// \brief Index of the current trajectory point + public: unsigned int pointIndex; + + /// \brief Ordered joints that need to be actuated to follow the current + /// trajectory + public: std::vector jointNames; + + /// \brief Trajectory defined in terms of temporal points, whose members are + /// ordered according to `jointNames` + public: std::vector points; +}; + +/// \brief Private data of the JointTrajectoryController plugin +class ignition::gazebo::systems::JointTrajectoryControllerPrivate +{ + /// \brief Get a list of enabled, unique, 1-axis joints of the model. If no + /// joint names are specified in the plugin configuration, all valid 1-axis + /// joints are returned + /// \param[in] _entity Entity of the model that the plugin is being + /// configured for + /// \param[in] _sdf SDF reference used to determine enabled joints + /// \param[in] _ecm Ignition Entity Component Manager + /// \return List of entities containinig all enabled joints + public: std::vector GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const; + + /// \brief Callback for joint trajectory subscription + /// \param[in] _msg A new message describing a joint trajectory that needs + /// to be followed + public: void JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg); + + /// \brief Reset internals of the plugin, without affecting already created + /// components + public: void Reset(); + + /// \brief Ignition communication node + public: transport::Node node; + + /// \brief Publisher of the progress for currently followed trajectory + public: transport::Node::Publisher progressPub; + + /// \brief Map of actuated joints, where the first entry of pair is the name + /// of the joint + public: std::map actuatedJoints; + + /// \brief Mutex projecting trajectory + public: std::mutex trajectoryMutex; + + /// \brief Information about trajectory that should be followed + public: Trajectory trajectory; + + /// \brief Flag that determines whether to use message header timestamp as + /// the trajectory start, where simulation time at the beginning of execution + /// is used otherwise + public: bool useHeaderStartTime; + + /// \brief Flag that determines if all components required for control are + /// already setup + public: bool componentSetupFinished; +}; + +///////////////////////////////// +/// JointTrajectoryController /// +///////////////////////////////// + +////////////////////////////////////////////////// +JointTrajectoryController::JointTrajectoryController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void JointTrajectoryController::Configure( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager & /*_eventManager*/) +{ + // Make sure the controller is attached to a valid model + const auto model = Model(_entity); + if (!model.Valid(_ecm)) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] is not a model. Please make sure that" + " JointTrajectoryController is attached to a valid model.\n"; + return; + } + ignmsg << "[JointTrajectoryController] Setting up controller for [" + << model.Name(_ecm) << "(Entity=" << _entity << ")].\n"; + + // Get list of enabled joints + const auto enabledJoints = this->dataPtr->GetEnabledJoints(_entity, + _sdf, + _ecm); + + // For each enabled joint, parse all of its parameters from SDF + auto jointParameters = JointParameters::ParseAll(_sdf, _ecm, enabledJoints); + + // Iterate over all enabled joints and create/configure them + for (const auto &jointEntity : enabledJoints) + { + const auto jointName = + _ecm.Component(jointEntity)->Data(); + this->dataPtr->actuatedJoints[jointName] = + ActuatedJoint(jointEntity, jointParameters[jointName]); + ignmsg << "[JointTrajectoryController] Configured joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + } + + // Make sure at least one joint is configured + if (this->dataPtr->actuatedJoints.empty()) + { + ignerr << "[JointTrajectoryController] Failed to initialize because [" + << model.Name(_ecm) << "(Entity=" << _entity + << ")] has no supported joints.\n"; + return; + } + + // Get additional parameters from SDF + if (_sdf->HasAttribute("use_header_start_time")) + { + this->dataPtr->useHeaderStartTime = + _sdf->Get("use_header_start_time"); + } + else + { + this->dataPtr->useHeaderStartTime = false; + } + + // Subscribe to joint trajectory commands + auto trajectoryTopic = _sdf->Get("topic"); + if (trajectoryTopic.empty()) + { + // If not specified, use the default topic based on model name + trajectoryTopic = "/model/" + model.Name(_ecm) + "/joint_trajectory"; + } + // Make sure the topic is valid + const auto validTrajectoryTopic = transport::TopicUtils::AsValidTopic( + trajectoryTopic); + if (validTrajectoryTopic.empty()) + { + ignerr << "[JointTrajectoryController] Cannot subscribe to invalid topic [" + << trajectoryTopic << "].\n"; + return; + } + // Subscribe + ignmsg << "[JointTrajectoryController] Subscribing to joint trajectory" + " commands on topic [" << validTrajectoryTopic << "].\n"; + this->dataPtr->node.Subscribe( + validTrajectoryTopic, + &JointTrajectoryControllerPrivate::JointTrajectoryCallback, + this->dataPtr.get()); + + // Advertise progress + const auto progressTopic = validTrajectoryTopic + "_progress"; + ignmsg << "[JointTrajectoryController] Advertising joint trajectory progress" + " on topic [" << progressTopic << "].\n"; + this->dataPtr->progressPub = + this->dataPtr->node.Advertise(progressTopic); +} + +////////////////////////////////////////////////// +void JointTrajectoryController::PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("JointTrajectoryController::PreUpdate"); + + // Create required components for each joint (only once) + if (!this->dataPtr->componentSetupFinished) + { + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + ActuatedJoint *joint = &actuatedJoint.second; + joint->SetupComponents(_ecm); + } + this->dataPtr->componentSetupFinished = true; + } + + // Reset plugin if jump back in time is detected + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignmsg << "[JointTrajectoryController] Resetting plugin because jump back" + " in time [" + << std::chrono::duration_cast(_info.dt).count() + << " s] was detected.\n"; + this->dataPtr->Reset(); + } + + // Nothing else to do if paused + if (_info.paused) + { + return; + } + + // Update joint targets based on the current trajectory + { + auto isTargetUpdateRequired = false; + + // Lock mutex before accessing trajectory + std::lock_guard lock(this->dataPtr->trajectoryMutex); + + if (this->dataPtr->trajectory.status == Trajectory::New) + { + // Set trajectory start time if not set before + if (this->dataPtr->trajectory.startTime.count() == 0) + { + this->dataPtr->trajectory.startTime = _info.simTime; + this->dataPtr->trajectory.status = Trajectory::Active; + } + + // If the new trajectory has no points, consider it reached + if (this->dataPtr->trajectory.points.empty()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Update is always needed for a new trajectory + isTargetUpdateRequired = true; + } + + if (this->dataPtr->trajectory.status == Trajectory::Active) + { + // Determine what point needs to be reached at the current time + if (this->dataPtr->trajectory.UpdateCurrentPoint(_info.simTime)) + { + // Update is needed if point was updated + isTargetUpdateRequired = true; + } + } + + // Update the target for each joint that is defined in the + // trajectory, if needed + if (isTargetUpdateRequired && + this->dataPtr->trajectory.status != Trajectory::Reached) + { + const auto targetPoint = + this->dataPtr->trajectory.points[this->dataPtr->trajectory + .pointIndex]; + for (auto jointIndex = 0u; + jointIndex < this->dataPtr->trajectory.jointNames.size(); + ++jointIndex) + { + const auto jointName = this->dataPtr->trajectory.jointNames[jointIndex]; + if (this->dataPtr->actuatedJoints.count(jointName) == 0) + { + // Warning about unconfigured joint is already logged above + continue; + } + auto *joint = &this->dataPtr->actuatedJoints[jointName]; + joint->SetTarget(targetPoint, jointIndex); + } + + // If there are no more points after the current one, set the trajectory + // to Reached + if (this->dataPtr->trajectory.IsGoalReached()) + { + this->dataPtr->trajectory.status = Trajectory::Reached; + } + + // Publish current progress of the trajectory + ignition::msgs::Float progressMsg; + progressMsg.set_data(this->dataPtr->trajectory.ComputeProgress()); + this->dataPtr->progressPub.Publish(progressMsg); + } + } + + // Control loop + for (auto &actuatedJoint : this->dataPtr->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + joint->Update(_ecm, _info.dt); + } +} + +//////////////////////////////////////// +/// JointTrajectoryControllerPrivate /// +//////////////////////////////////////// + +////////////////////////////////////////////////// +std::vector JointTrajectoryControllerPrivate::GetEnabledJoints( + const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm) const +{ + std::vector output; + + // Get list of user-enabled joint names. If empty, enable all 1-axis joints + const auto enabledJoints = JointParameters::Parse(_sdf, + "joint_name"); + + // Get list of joint entities of the model + // If there are joints explicitely enabled by the user, get only those + std::vector jointEntities; + if (!enabledJoints.empty()) + { + for (const auto &enabledJointName : enabledJoints) + { + auto enabledJointEntity = _ecm.ChildrenByComponents( + _entity, components::Joint(), components::Name(enabledJointName)); + // Check that model has exactly one joint that matches the name + if (enabledJointEntity.empty()) + { + ignerr << "[JointTrajectoryController] Model does not contain joint [" + << enabledJointName << "], which was explicitly enabled.\n"; + continue; + } + else if (enabledJointEntity.size() > 1) + { + ignwarn << "[JointTrajectoryController] Model has " + << enabledJointEntity.size() << " duplicate joints named [" + << enabledJointName << "]. Only the first (Entity=" + << enabledJointEntity[0] << ") will be configured.\n"; + } + // Add entity to the list of enabled joints + jointEntities.push_back(enabledJointEntity[0]); + } + } + else + { + jointEntities = _ecm.ChildrenByComponents(_entity, components::Joint()); + } + + // Iterate over all joints and verify whether they can be enabled or not + for (const auto &jointEntity : jointEntities) + { + const auto jointName = _ecm.Component( + jointEntity)->Data(); + + // Ignore duplicate joints + for (const auto &actuatedJoint : this->actuatedJoints) + { + if (actuatedJoint.second.entity == jointEntity) + { + ignwarn << "[JointTrajectoryController] Ignoring duplicate joint [" + << jointName << "(Entity=" << jointEntity << ")].\n"; + continue; + } + } + + // Make sure the joint type is supported, i.e. it has exactly one + // actuated axis + const auto *jointType = _ecm.Component(jointEntity); + switch (jointType->Data()) + { + case sdf::JointType::PRISMATIC: + case sdf::JointType::REVOLUTE: + case sdf::JointType::CONTINUOUS: + case sdf::JointType::GEARBOX: + { + // Supported joint type + break; + } + case sdf::JointType::FIXED: + { + igndbg << "[JointTrajectoryController] Fixed joint [" << jointName + << "(Entity=" << jointEntity << ")] is skipped.\n"; + continue; + } + case sdf::JointType::REVOLUTE2: + case sdf::JointType::SCREW: + case sdf::JointType::BALL: + case sdf::JointType::UNIVERSAL: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity + << ")] is of unsupported type. Only joints with a single axis" + " are supported.\n"; + continue; + } + default: + { + ignwarn << "[JointTrajectoryController] Joint [" << jointName + << "(Entity=" << jointEntity << ")] is of unknown type.\n"; + continue; + } + } + output.push_back(jointEntity); + } + + return output; +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::JointTrajectoryCallback( + const ignition::msgs::JointTrajectory &_msg) +{ + // Make sure the message is valid + if (_msg.joint_names_size() == 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message does not" + " contain any joint names.\n"; + return; + } + + // Warn user that accelerations are currently ignored if the first point + // contains them + if (_msg.points(0).accelerations_size() > 0) + { + ignwarn << "[JointTrajectoryController] JointTrajectory message contains" + " acceleration commands, which are currently ignored.\n"; + } + + // Lock mutex guarding the trajectory + std::lock_guard lock(this->trajectoryMutex); + + if (this->trajectory.status != Trajectory::Reached) + { + ignwarn << "[JointTrajectoryController] A new JointTrajectory message was" + " received while executing a previous trajectory.\n"; + } + + // Get start time of the trajectory from message header if desired + // If not enabled or there is no header, set start time to 0 and determine + // it later from simTime + if (this->useHeaderStartTime && _msg.has_header()) + { + if (_msg.header().has_stamp()) + { + const auto stamp = _msg.header().stamp(); + this->trajectory.startTime = std::chrono::seconds(stamp.sec()) + + std::chrono::nanoseconds(stamp.nsec()); + } + } + else + { + this->trajectory.startTime = std::chrono::nanoseconds(0); + } + + // Reset for a new trajectory + this->trajectory.Reset(); + + // Extract joint names and points + for (const auto &joint_name : _msg.joint_names()) + { + this->trajectory.jointNames.push_back(joint_name); + } + for (const auto &point : _msg.points()) + { + this->trajectory.points.push_back(point); + } +} + +////////////////////////////////////////////////// +void JointTrajectoryControllerPrivate::Reset() +{ + for (auto &actuatedJoint : this->actuatedJoints) + { + auto *joint = &actuatedJoint.second; + // Reset joint target + joint->ResetTarget(); + // Reset PIDs + joint->ResetPIDs(); + } + + // Reset trajectory + this->trajectory.Reset(); +} + +/////////////////////// +/// JointParameters /// +/////////////////////// + +////////////////////////////////////////////////// +std::map JointParameters::ParseAll( + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + std::vector _enabledJoints) +{ + std::map output; + + const auto initialPositionAll = JointParameters::Parse(_sdf, + "initial_position"); + + const auto positionPGainAll = JointParameters::Parse(_sdf, + "position_p_gain"); + const auto positionIGainAll = JointParameters::Parse(_sdf, + "position_i_gain"); + const auto positionDGainAll = JointParameters::Parse(_sdf, + "position_d_gain"); + const auto positionIMinAll = JointParameters::Parse(_sdf, + "position_i_min"); + const auto positionIMaxAll = JointParameters::Parse(_sdf, + "position_i_max"); + const auto positionCmdMinAll = JointParameters::Parse(_sdf, + "position_cmd_min"); + const auto positionCmdMaxAll = JointParameters::Parse(_sdf, + "position_cmd_max"); + const auto positionCmdOffsetAll = JointParameters::Parse(_sdf, + "position_cmd_offset"); + + const auto velocityPGainAll = JointParameters::Parse(_sdf, + "velocity_p_gain"); + const auto velocityIGainAll = JointParameters::Parse(_sdf, + "velocity_i_gain"); + const auto velocityDGainAll = JointParameters::Parse(_sdf, + "velocity_d_gain"); + const auto velocityIMinAll = JointParameters::Parse(_sdf, + "velocity_i_min"); + const auto velocityIMaxAll = JointParameters::Parse(_sdf, + "velocity_i_max"); + const auto velocityCmdMinAll = JointParameters::Parse(_sdf, + "velocity_cmd_min"); + const auto velocityCmdMaxAll = JointParameters::Parse(_sdf, + "velocity_cmd_max"); + const auto velocityCmdOffsetAll = JointParameters::Parse(_sdf, + "velocity_cmd_offset"); + + for (std::size_t i = 0; i < _enabledJoints.size(); ++i) + { + JointParameters params; + params.initialPosition = params.NthElementOr(initialPositionAll, i, + params.initialPositionDefault); + + params.positionPID.pGain = params.NthElementOr(positionPGainAll, i, + params.positionPID.pGainDefault); + params.positionPID.iGain = params.NthElementOr(positionIGainAll, i, + params.positionPID.iGainDefault); + params.positionPID.dGain = params.NthElementOr(positionDGainAll, i, + params.positionPID.dGainDefault); + params.positionPID.iMin = params.NthElementOr(positionIMinAll, i, + params.positionPID.iMinDefault); + params.positionPID.iMax = params.NthElementOr(positionIMaxAll, i, + params.positionPID.iMaxDefault); + params.positionPID.cmdMin = params.NthElementOr(positionCmdMinAll, i, + params.positionPID.cmdMinDefault); + params.positionPID.cmdMax = params.NthElementOr(positionCmdMaxAll, i, + params.positionPID.cmdMaxDefault); + params.positionPID.cmdOffset = params.NthElementOr(positionCmdOffsetAll, i, + params.positionPID.cmdOffsetDefault); + + params.velocityPID.pGain = params.NthElementOr(velocityPGainAll, i, + params.velocityPID.pGainDefault); + params.velocityPID.iGain = params.NthElementOr(velocityIGainAll, i, + params.velocityPID.iGainDefault); + params.velocityPID.dGain = params.NthElementOr(velocityDGainAll, i, + params.velocityPID.dGainDefault); + params.velocityPID.iMin = params.NthElementOr(velocityIMinAll, i, + params.velocityPID.iMinDefault); + params.velocityPID.iMax = params.NthElementOr(velocityIMaxAll, i, + params.velocityPID.iMaxDefault); + params.velocityPID.cmdMin = params.NthElementOr(velocityCmdMinAll, i, + params.velocityPID.cmdMinDefault); + params.velocityPID.cmdMax = params.NthElementOr(velocityCmdMaxAll, i, + params.velocityPID.cmdMaxDefault); + params.velocityPID.cmdOffset = params.NthElementOr(velocityCmdOffsetAll, i, + params.velocityPID.cmdOffsetDefault); + + const auto jointName = _ecm.Component( + _enabledJoints[i])->Data(); + output[jointName] = params; + } + + return output; +} + +////////////////////////////////////////////////// +template +std::vector JointParameters::Parse( + const std::shared_ptr &_sdf, + const std::string &_parameterName) +{ + std::vector output; + + if (_sdf->HasElement(_parameterName)) + { + sdf::ElementPtr param = const_cast( + _sdf.get())->GetElement(_parameterName); + while (param) + { + output.push_back(param->Get()); + param = param->GetNextElement(_parameterName); + } + } + + return output; +} + +//////////////////////////////////////////////// +template +T JointParameters::NthElementOr(const std::vector &_vec, + const size_t &_index, + const T &_alternative_value) +{ + if (_index < _vec.size()) + { + return _vec[_index]; + } + else + { + return _alternative_value; + } +} + +///////////////////// +/// ActuatedJoint /// +///////////////////// + +////////////////////////////////////////////////// +ActuatedJoint::ActuatedJoint(const Entity &_entity, + const JointParameters &_params) +{ + this->entity = _entity; + + this->initialPosition = _params.initialPosition; + this->target.position = _params.initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; + + this->pids.position = ignition::math::PID(_params.positionPID.pGain, + _params.positionPID.iGain, + _params.positionPID.dGain, + _params.positionPID.iMax, + _params.positionPID.iMin, + _params.positionPID.cmdMax, + _params.positionPID.cmdMin, + _params.positionPID.cmdOffset); + + this->pids.velocity = ignition::math::PID(_params.velocityPID.pGain, + _params.velocityPID.iGain, + _params.velocityPID.dGain, + _params.velocityPID.iMax, + _params.velocityPID.iMin, + _params.velocityPID.cmdMax, + _params.velocityPID.cmdMin, + _params.velocityPID.cmdOffset); + + igndbg << "[JointTrajectoryController] Parameters for joint (Entity=" + << _entity << "):\n" + << "initial_position: [" << _params.initialPosition << "]\n" + << "position_p_gain: [" << _params.positionPID.pGain << "]\n" + << "position_i_gain: [" << _params.positionPID.iGain << "]\n" + << "position_d_gain: [" << _params.positionPID.dGain << "]\n" + << "position_i_min: [" << _params.positionPID.iMax << "]\n" + << "position_i_max: [" << _params.positionPID.iMax << "]\n" + << "position_cmd_min: [" << _params.positionPID.cmdMin << "]\n" + << "position_cmd_max: [" << _params.positionPID.cmdMax << "]\n" + << "position_cmd_offset: [" << _params.positionPID.cmdOffset << "]\n" + << "velocity_p_gain: [" << _params.velocityPID.pGain << "]\n" + << "velocity_i_gain: [" << _params.velocityPID.iGain << "]\n" + << "velocity_d_gain: [" << _params.velocityPID.dGain << "]\n" + << "velocity_i_min: [" << _params.velocityPID.iMax << "]\n" + << "velocity_i_max: [" << _params.velocityPID.iMax << "]\n" + << "velocity_cmd_min: [" << _params.velocityPID.cmdMin << "]\n" + << "velocity_cmd_max: [" << _params.velocityPID.cmdMax << "]\n" + << "velocity_cmd_offset: [" << _params.velocityPID.cmdOffset << "]\n"; +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetupComponents( + ignition::gazebo::EntityComponentManager &_ecm) const +{ + // Create JointPosition component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointPosition()); + } + + // Create JointVelocity component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointVelocity()); + } + + // Create JointForceCmd component if one does not exist + if (nullptr == _ecm.Component(this->entity)) + { + _ecm.CreateComponent(this->entity, components::JointForceCmd({0.0})); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::SetTarget( + const ignition::msgs::JointTrajectoryPoint &_targetPoint, + const size_t &_jointIndex) +{ + if ((signed)_jointIndex < _targetPoint.positions_size()) + { + this->target.position = _targetPoint.positions(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.velocities_size()) + { + this->target.velocity = _targetPoint.velocities(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.accelerations_size()) + { + this->target.acceleration = _targetPoint.accelerations(_jointIndex); + } + if ((signed)_jointIndex < _targetPoint.effort_size()) + { + this->target.effort = _targetPoint.effort(_jointIndex); + } +} + +////////////////////////////////////////////////// +void ActuatedJoint::Update(ignition::gazebo::EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_dt) +{ + // Get JointPosition and JointVelocity components + const auto jointPositionComponent = _ecm.Component( + this->entity); + const auto jointVelocityComponent = _ecm.Component( + this->entity); + + // Compute control errors and force for each PID controller + double forcePosition = 0.0, forceVelocity = 0.0; + if (!jointPositionComponent->Data().empty()) + { + double errorPosition = jointPositionComponent->Data()[0] - + this->target.position; + forcePosition = this->pids.position.Update(errorPosition, _dt); + } + if (!jointVelocityComponent->Data().empty()) + { + double errorVelocity = jointVelocityComponent->Data()[0] - + this->target.velocity; + forceVelocity = this->pids.velocity.Update(errorVelocity, _dt); + } + + // Sum all forces + const double force = forcePosition + forceVelocity + this->target.effort; + + // Get JointForceCmd component and apply command force + auto jointForceCmdComponent = _ecm.Component( + this->entity); + jointForceCmdComponent->Data()[0] = force; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetTarget() +{ + this->target.position = this->initialPosition; + this->target.velocity = 0.0; + this->target.acceleration = 0.0; + this->target.effort = 0.0; +} + +////////////////////////////////////////////////// +void ActuatedJoint::ResetPIDs() +{ + this->pids.position.Reset(); + this->pids.velocity.Reset(); +} + +////////////////// +/// Trajectory /// +////////////////// + +////////////////////////////////////////////////// +bool Trajectory::UpdateCurrentPoint( + const std::chrono::steady_clock::duration &_simTime) +{ + bool isUpdated = false; + + const auto trajectoryTime = _simTime - this->startTime; + while (true) + { + // Break if end of trajectory is reached (there are no more points after + // the current one) + if (this->IsGoalReached()) + { + break; + } + + // Break if point needs to be followed + const auto pointTFS = this->points[this->pointIndex].time_from_start(); + const auto pointTime = std::chrono::seconds(pointTFS.sec()) + + std::chrono::nanoseconds(pointTFS.nsec()); + if (pointTime >= trajectoryTime) + { + break; + } + + // Otherwise increment and try again (joint targets need to be updated) + ++this->pointIndex; + isUpdated = true; + }; + + // Return true if a point index was updated + return isUpdated; +} + +////////////////////////////////////////////////// +bool Trajectory::IsGoalReached() const +{ + return this->pointIndex + 1 >= this->points.size(); +} + +////////////////////////////////////////////////// +float Trajectory::ComputeProgress() const +{ + if (this->points.size() == 0) + { + return 1.0; + } + else + { + return static_cast(this->pointIndex + 1) / + static_cast(this->points.size()); + } +} + +////////////////////////////////////////////////// +void Trajectory::Reset() +{ + this->status = Trajectory::New; + this->pointIndex = 0; + this->jointNames.clear(); + this->points.clear(); +} + +// Register plugin +IGNITION_ADD_PLUGIN(JointTrajectoryController, + ignition::gazebo::System, + JointTrajectoryController::ISystemConfigure, + JointTrajectoryController::ISystemPreUpdate) +IGNITION_ADD_PLUGIN_ALIAS( + JointTrajectoryController, + "ignition::gazebo::systems::JointTrajectoryController") diff --git a/src/systems/joint_trajectory_controller/JointTrajectoryController.hh b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh new file mode 100644 index 0000000000..fb60279d1d --- /dev/null +++ b/src/systems/joint_trajectory_controller/JointTrajectoryController.hh @@ -0,0 +1,164 @@ +/* + * 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_JOINT_TRAJECTORY_CONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_JOINT_TRAJECTORY_CONTROLLER_HH_ + +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class JointTrajectoryControllerPrivate; + + /// \brief Joint trajectory controller, which can be attached to a model with + /// reference to one or more 1-axis joints in order to follow a trajectory. + /// + /// Joint trajectories can be sent to this plugin via Ignition Transport. + /// The default topic name is "/model/${MODEL_NAME}/joint_trajectory" that + /// can be configured with the `` system parameter. + /// + /// This topic accepts ignition::msgs::JointTrajectory messages that represent + /// a full trajectory, defined as temporal `points` with their fields ordered + /// according to `joint_names` field. The fields under `points` are + /// `positions` - Controlled by position PID controller for each joint + /// `velocities` - Controlled by velocity PID controller for each joint + /// `accelerations` - This field is currently ignored + /// `effort` - Controlled directly for each joint + /// `time_from_start` - Temporal information about the point + /// + /// Forces/torques from `position`, `velocity` and `effort` are summed and + /// applied to the joint. Each PID controller can be configured with system + /// parameters described below. + /// + /// Input trajectory can be produced by a motion planning framework such as + /// MoveIt2. For smooth execution of the trajectory, its points should to be + /// interpolated before sending them via Ignition Transport (interpolation + /// might already be implemented in the motion planning framework of your + /// choice). + /// + /// The progress of the current trajectory can be tracked on topic whose name + /// is derived as `_progress`. This progress is indicated in the range + /// of (0.0, 1.0] and is currently based purely on `time_from_start` contained + /// in the trajectory points. + /// + /// ## System Parameters + /// + /// `` The name of the topic that this plugin subscribes to. + /// Optional parameter. + /// Defaults to "/model/${MODEL_NAME}/joint_trajectory". + /// + /// `` If enabled, trajectory execution begins at the + /// timestamp contained in the header of received trajectory messages. + /// Optional parameter. + /// Defaults to false. + /// + /// `` Name of a joint to control. + /// This parameter can be specified multiple times, i.e. once for each joint. + /// Optional parameter. + /// Defaults to all 1-axis joints contained in the model SDF (order is kept). + /// + /// `` Initial position of a joint (for position control). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// Defaults to 0 for all joints. + /// + /// `<%s_p_gain>` The proportional gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_gain>` The integral gain of the PID. Optional parameter. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_d_gain>` The derivative gain of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (disabled). + /// + /// `<%s_i_min>` The integral lower limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_i_max>` The integral upper limit of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_min>` Output min value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no limit if higher than `%s_i_max`). + /// + /// `<%s_cmd_max>` Output max value of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is -1 (no limit if lower than `%s_i_min`). + /// + /// `<%s_cmd_offset>` Command offset (feed-forward) of the PID. + /// Substitute '%s' for "position" or "velocity" (e.g. "position_p_gain"). + /// This parameter can be specified multiple times. Follows joint_name order. + /// Optional parameter. + /// The default value is 0 (no offset). + class JointTrajectoryController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: JointTrajectoryController(); + + /// \brief Destructor + public: ~JointTrajectoryController() 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; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} // namespace systems +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition + +#endif diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index c061bbe997..e7b557e44d 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -183,7 +183,7 @@ void LogPlayback::Configure(const Entity &, EntityComponentManager &_ecm, EventManager &_eventMgr) { // Get directory paths from SDF - this->dataPtr->logPath = _sdf->Get("path"); + this->dataPtr->logPath = _sdf->Get("playback_path"); this->dataPtr->eventManager = &_eventMgr; diff --git a/src/systems/particle_emitter/CMakeLists.txt b/src/systems/particle_emitter/CMakeLists.txt new file mode 100644 index 0000000000..9e75a90abf --- /dev/null +++ b/src/systems/particle_emitter/CMakeLists.txt @@ -0,0 +1,9 @@ +gz_add_system(particle-emitter + SOURCES + ParticleEmitter.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_emitter/ParticleEmitter.cc b/src/systems/particle_emitter/ParticleEmitter.cc new file mode 100644 index 0000000000..697b260544 --- /dev/null +++ b/src/systems/particle_emitter/ParticleEmitter.cc @@ -0,0 +1,336 @@ +/* + * 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 +#include +#include +#include +#include "ParticleEmitter.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +// Private data class. +class ignition::gazebo::systems::ParticleEmitterPrivate +{ + /// \brief Callback for receiving particle emitter commands. + /// \param[in] _msg Particle emitter message. + public: void OnCmd(const ignition::msgs::ParticleEmitter &_msg); + + /// \brief The particle emitter parsed from SDF. + public: ignition::msgs::ParticleEmitter emitter; + + /// \brief The transport node. + public: ignition::transport::Node node; + + /// \brief Particle emitter entity. + public: Entity emitterEntity{kNullEntity}; + + /// \brief The particle emitter command requested externally. + public: ignition::msgs::ParticleEmitter userCmd; + + public: bool newDataReceived = false; + + /// \brief A mutex to protect the user command. + public: std::mutex mutex; +}; + +////////////////////////////////////////////////// +void ParticleEmitterPrivate::OnCmd(const msgs::ParticleEmitter &_msg) +{ + std::lock_guard lock(this->mutex); + this->userCmd = _msg; + this->newDataReceived = true; +} + +////////////////////////////////////////////////// +ParticleEmitter::ParticleEmitter() + : System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void ParticleEmitter::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + Model model = Model(_entity); + + if (!model.Valid(_ecm)) + { + ignerr << "ParticleEmitter plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + // Create a particle emitter entity. + this->dataPtr->emitterEntity = _ecm.CreateEntity(); + if (this->dataPtr->emitterEntity == kNullEntity) + { + ignerr << "Failed to create a particle emitter entity" << std::endl; + return; + } + + // allow_renaming + bool allowRenaming = false; + if (_sdf->HasElement("allow_renaming")) + allowRenaming = _sdf->Get("allow_renaming"); + + // Name. + std::string name = "particle_emitter_entity_" + + std::to_string(this->dataPtr->emitterEntity); + if (_sdf->HasElement("emitter_name")) + { + std::set emitterNames; + std::string emitterName = _sdf->Get("emitter_name"); + + // check to see if name is already taken + _ecm.Each( + [&emitterNames](const Entity &, const components::Name *_name, + const components::ParticleEmitter *) + { + emitterNames.insert(_name->Data()); + return true; + }); + + name = emitterName; + + // rename emitter if needed + if (emitterNames.find(emitterName) != emitterNames.end()) + { + if (!allowRenaming) + { + ignwarn << "Entity named [" << name + << "] already exists and " + << "[allow_renaming] is false. Entity not spawned." + << std::endl; + return; + } + int counter = 0; + while (emitterNames.find(name) != emitterNames.end()) + { + name = emitterName + "_" + std::to_string(++counter); + } + ignmsg << "Entity named [" << emitterName + << "] already exists. Renaming it to " << name << std::endl; + } + } + this->dataPtr->emitter.set_name(name); + + // Type. The default type is point. + this->dataPtr->emitter.set_type( + ignition::msgs::ParticleEmitter_EmitterType_POINT); + std::string type = _sdf->Get("type", "point").first; + if (type == "box") + { + this->dataPtr->emitter.set_type( + ignition::msgs::ParticleEmitter_EmitterType_BOX); + } + else if (type == "cylinder") + { + this->dataPtr->emitter.set_type( + ignition::msgs::ParticleEmitter_EmitterType_CYLINDER); + } + else if (type == "ellipsoid") + { + this->dataPtr->emitter.set_type( + ignition::msgs::ParticleEmitter_EmitterType_ELLIPSOID); + } + else if (type != "point") + { + ignerr << "Unknown emitter type [" << type << "]. Using [point] instead" + << std::endl; + } + + // Pose. + ignition::math::Pose3d pose = + _sdf->Get("pose"); + ignition::msgs::Set(this->dataPtr->emitter.mutable_pose(), pose); + + // Size. + ignition::math::Vector3d size = ignition::math::Vector3d::One; + if (_sdf->HasElement("size")) + size = _sdf->Get("size"); + ignition::msgs::Set(this->dataPtr->emitter.mutable_size(), size); + + // Rate. + this->dataPtr->emitter.set_rate(_sdf->Get("rate", 10).first); + + // Duration. + this->dataPtr->emitter.set_duration(_sdf->Get("duration", 0).first); + + // Emitting. + this->dataPtr->emitter.set_emitting(_sdf->Get("emitting", false).first); + + // Particle size. + size = ignition::math::Vector3d::One; + if (_sdf->HasElement("particle_size")) + size = _sdf->Get("particle_size"); + ignition::msgs::Set(this->dataPtr->emitter.mutable_particle_size(), size); + + // Lifetime. + this->dataPtr->emitter.set_lifetime(_sdf->Get("lifetime", 5).first); + + // Material. + if (_sdf->HasElement("material")) + { + auto materialElem = _sdf->GetElementImpl("material"); + sdf::Material material; + material.Load(materialElem); + ignition::msgs::Material materialMsg = convert(material); + this->dataPtr->emitter.mutable_material()->CopyFrom(materialMsg); + } + + // Min velocity. + this->dataPtr->emitter.set_min_velocity( + _sdf->Get("min_velocity", 1).first); + + // Max velocity. + this->dataPtr->emitter.set_max_velocity( + _sdf->Get("max_velocity", 1).first); + + // Color start. + ignition::math::Color color = ignition::math::Color::White; + if (_sdf->HasElement("color_start")) + color = _sdf->Get("color_start"); + ignition::msgs::Set(this->dataPtr->emitter.mutable_color_start(), color); + + // Color end. + color = ignition::math::Color::White; + if (_sdf->HasElement("color_end")) + color = _sdf->Get("color_end"); + ignition::msgs::Set(this->dataPtr->emitter.mutable_color_end(), color); + + // Scale rate. + this->dataPtr->emitter.set_scale_rate( + _sdf->Get("scale_rate", 1).first); + + // Color range image. + if (_sdf->HasElement("color_range_image")) + { + auto modelPath = _ecm.ComponentData(_entity); + auto colorRangeImagePath = _sdf->Get("color_range_image"); + auto path = asFullPath(colorRangeImagePath, modelPath.value()); + + common::SystemPaths systemPaths; + systemPaths.SetFilePathEnv(kResourcePathEnv); + auto absolutePath = systemPaths.FindFile(path); + + this->dataPtr->emitter.set_color_range_image(absolutePath); + } + + igndbg << "Loading particle emitter:" << std::endl + << this->dataPtr->emitter.DebugString() << std::endl; + + // Create components. + SdfEntityCreator sdfEntityCreator(_ecm, _eventMgr); + sdfEntityCreator.SetParent(this->dataPtr->emitterEntity, _entity); + + _ecm.CreateComponent(this->dataPtr->emitterEntity, + components::Name(this->dataPtr->emitter.name())); + + _ecm.CreateComponent(this->dataPtr->emitterEntity, + components::ParticleEmitter(this->dataPtr->emitter)); + + _ecm.CreateComponent(this->dataPtr->emitterEntity, components::Pose(pose)); + + // Advertise the topic to receive particle emitter commands. + const std::string kDefaultTopic = + "/model/" + model.Name(_ecm) + "/particle_emitter/" + name; + std::string topic = _sdf->Get("topic", kDefaultTopic).first; + if (!this->dataPtr->node.Subscribe( + topic, &ParticleEmitterPrivate::OnCmd, this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << topic << "]. " + << "Particle emitter will not receive updates." << std::endl; + return; + } + igndbg << "Subscribed to " << topic << " for receiving particle emitter " + << "updates" << std::endl; +} + +////////////////////////////////////////////////// +void ParticleEmitter::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("ParticleEmitter::PreUpdate"); + + std::lock_guard lock(this->dataPtr->mutex); + + if (!this->dataPtr->newDataReceived) + return; + + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->newDataReceived = false; + + // Create component. + auto emitterComp = _ecm.Component( + this->dataPtr->emitterEntity); + if (!emitterComp) + { + _ecm.CreateComponent( + this->dataPtr->emitterEntity, + components::ParticleEmitterCmd(this->dataPtr->userCmd)); + } + else + { + emitterComp->Data() = this->dataPtr->userCmd; + + // 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(this->dataPtr->emitterEntity, + components::ParticleEmitterCmd::typeId, + ComponentState::OneTimeChange); + } + + igndbg << "New ParticleEmitterCmd component created" << std::endl; +} + +IGNITION_ADD_PLUGIN(ParticleEmitter, + ignition::gazebo::System, + ParticleEmitter::ISystemConfigure, + ParticleEmitter::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ParticleEmitter, + "ignition::gazebo::systems::ParticleEmitter") diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh new file mode 100644 index 0000000000..7a9ab1e278 --- /dev/null +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -0,0 +1,141 @@ +/* + * 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_EMITTER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_PARTICLE_EMITTER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class ParticleEmitterPrivate; + + /// \brief A system for creating a particle emitter. + /// + /// System parameters + /// + /// ``: Unique name for the particle emitter. The name will be + /// automatically generated if this parameter is not set. + /// ``: Rename the particle emitter if one with the same name + /// already exists. Default is false. + /// ``: The emitter type (point, box, cylinder, ellipsoid). + /// Default value is point. + /// ``: The pose of the emitter. Default value is {0, 0, 0, 0, 0, 0}. + /// ``: The size of the emitter where the particles are sampled. + /// Default value is (1, 1, 1). + /// Note that the interpretation of the emitter area varies + /// depending on the emmiter type: + /// - point: The area is ignored. + /// - box: The area is interpreted as width X height X depth. + /// - cylinder: The area is interpreted as the bounding box of the + /// cilinder. The cylinder is oriented along the + /// Z-axis. + /// - ellipsoid: The area is interpreted as the bounding box of an + /// ellipsoid shaped area, i.e. a sphere or + /// squashed-sphere area. The parameters are again + /// identical to EM_BOX, except that the dimensions + /// describe the widest points along each of the + /// axes. + /// ``: How many particles per second should be emitted. + /// Default value is 10. + /// `: The number of seconds the emitter is active. A value of 0 + /// means infinite duration. Default value is 0. + /// ``: This is used to turn on or off particle emission. + /// Default value is false. + /// ``: Set the particle dimensions (width, height, depth). + /// Default value is {1, 1, 1}. + /// ``: Set the number of seconds each particle will ’live’ for + /// before being destroyed. Default value is 5. + /// ``: Sets the material which all particles in the emitter will + /// use. + /// ``: Sets a minimum velocity for each particle (m/s). + /// Default value is 1. + /// ``: Sets a maximum velocity for each particle (m/s). + /// Default value is 1. + /// ``: Sets the starting color for all particle emitted. + /// The actual color will be interpolated between this color + /// and the one set under . + /// Color::White is the default color for the particles + /// unless a specific function is used. + /// To specify a color, RGB values should be passed in. + /// For example, to specify red, a user should enter: + /// 1 0 0 + /// Note that this function overrides the particle colors set + /// with . + /// ``: Sets the end color for all particle emitted. + /// The actual color will be interpolated between this color + /// and the one set under . + /// Color::White is the default color for the particles + /// unless a specific function is used (see color_start for + /// more information about defining custom colors with RGB + /// values). + /// Note that this function overrides the particle colors set + /// with . + /// ``: Sets the amount by which to scale the particles in both x + /// and y direction per second. Default value is 1. + /// ``: Sets the path to the color image used as an + /// affector. This affector modifies the color of + /// particles in flight. The colors are taken from a + /// specified image file. The range of color values + /// begins from the left side of the image and move to + /// the right over the lifetime of the particle, + /// therefore only the horizontal dimension of the + /// image is used. + /// Note that this function overrides the particle + /// colors set with and . + /// ``: Topic used to update particle emitter properties at runtime. + /// The default topic is + /// /model//particle_emitter/ + /// Note that the emitter id and name may not be changed. + /// See the examples/worlds/particle_emitter.sdf example world for + /// example usage. + class ParticleEmitter + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: ParticleEmitter(); + + // 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 e8f52800d2..3d641a9750 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -27,6 +27,7 @@ #include #include #include +#include #include #include diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 7e5094cc0d..6f78502cfb 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -293,12 +293,13 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, { _manager.State(*this->dataPtr->stepMsg.mutable_state(), {}, {}, true); } - // Otherwise publish just selected components + // Otherwise publish just periodic change components else { IGN_PROFILE("SceneBroadcast::PostUpdate UpdateState"); + auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, {components::Pose::typeId}); + {}, periodicComponents); } // Full state on demand diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index d973d94a60..a20b5c3280 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -78,6 +78,10 @@ class ignition::gazebo::systems::SensorsPrivate /// sea level public: double ambientTemperature = 288.15; + /// \brief Temperature gradient with respect to increasing altitude at sea + /// level in units of K/m. + public: double ambientTemperatureGradient = -0.0065; + /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera @@ -386,6 +390,8 @@ void Sensors::Configure(const Entity &/*_id*/, { auto atmosphereSdf = atmosphere->Data(); this->dataPtr->ambientTemperature = atmosphereSdf.Temperature().Kelvin(); + this->dataPtr->ambientTemperatureGradient = + atmosphereSdf.TemperatureGradient(); } // Set render engine if specified from command line @@ -406,6 +412,17 @@ void Sensors::Configure(const Entity &/*_id*/, this->dataPtr->Run(); } +////////////////////////////////////////////////// +void Sensors::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("Sensors::Update"); + if (this->dataPtr->running && this->dataPtr->initialized) + { + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + } +} + ////////////////////////////////////////////////// void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) @@ -564,6 +581,27 @@ std::string Sensors::CreateSensor(const Entity &_entity, if (nullptr != thermalSensor) { thermalSensor->SetAmbientTemperature(this->dataPtr->ambientTemperature); + + // temperature gradient is in kelvin per meter - typically change in + // temperature over change in altitude. However the implementation of + // thermal sensor in ign-sensors varies temperature for all objects in its + // view. So we will do an approximation based on camera view's vertical + // distance. + auto camSdf = _sdf.CameraSensor(); + double farClip = camSdf->FarClip(); + double angle = camSdf->HorizontalFov().Radian(); + double aspect = camSdf->ImageWidth() / camSdf->ImageHeight(); + double vfov = 2.0 * atan(tan(angle / 2.0) / aspect); + double height = tan(vfov / 2.0) * farClip * 2.0; + double tempRange = + std::fabs(this->dataPtr->ambientTemperatureGradient * height); + thermalSensor->SetAmbientTemperatureRange(tempRange); + + ignmsg << "Setting ambient temperature to " + << this->dataPtr->ambientTemperature << " Kelvin and gradient to " + << this->dataPtr->ambientTemperatureGradient << " K/m. " + << "The resulting temperature range is: " << tempRange + << " Kelvin." << std::endl; } return sensor->Name(); @@ -571,6 +609,7 @@ std::string Sensors::CreateSensor(const Entity &_entity, IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, + Sensors::ISystemUpdate, Sensors::ISystemPostUpdate ) diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index cb88b2ff03..3f988fa146 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -41,6 +41,7 @@ namespace systems class Sensors: public System, public ISystemConfigure, + public ISystemUpdate, public ISystemPostUpdate { /// \brief Constructor @@ -55,6 +56,10 @@ namespace systems EntityComponentManager &_ecm, EventManager &_eventMgr) final; + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + // Documentation inherited public: void PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) final; diff --git a/src/systems/thermal/CMakeLists.txt b/src/systems/thermal/CMakeLists.txt index ff3e7aa265..c6d6dfc3ba 100644 --- a/src/systems/thermal/CMakeLists.txt +++ b/src/systems/thermal/CMakeLists.txt @@ -5,3 +5,9 @@ gz_add_system(thermal ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ) +gz_add_system(thermal-sensor + SOURCES + ThermalSensor.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} +) diff --git a/src/systems/thermal/Thermal.cc b/src/systems/thermal/Thermal.cc index 2a0f346797..28bd118931 100644 --- a/src/systems/thermal/Thermal.cc +++ b/src/systems/thermal/Thermal.cc @@ -15,16 +15,23 @@ * */ -#include +#include "Thermal.hh" + +#include +#include + +#include +#include #include -#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Atmosphere.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/Util.hh" -#include "Thermal.hh" - using namespace ignition; using namespace gazebo; using namespace systems; @@ -48,14 +55,79 @@ void Thermal::Configure(const Entity &_entity, gazebo::EntityComponentManager &_ecm, gazebo::EventManager & /*_eventMgr*/) { - if (!_sdf->HasElement("temperature")) + const std::string temperatureTag = "temperature"; + const std::string heatSignatureTag = "heat_signature"; + const std::string minTempTag = "min_temp"; + const std::string maxTempTag = "max_temp"; + + if (_sdf->HasElement(temperatureTag) && _sdf->HasElement(heatSignatureTag)) + { + ignwarn << "Both <" << temperatureTag << "> and <" << heatSignatureTag + << "> were specified, but the thermal system only uses one. " + << "<" << heatSignatureTag << "> will be used.\n"; + } + + if (_sdf->HasElement(heatSignatureTag)) + { + std::string heatSignature = _sdf->Get(heatSignatureTag); + auto modelEntity = topLevelModel(_entity, _ecm); + auto modelPath = + _ecm.ComponentData(modelEntity); + auto path = common::findFile(asFullPath(heatSignature, modelPath.value())); + + // make sure the specified heat signature can be found + if (path.empty()) + { + ignerr << "Failed to load thermal system. Heat signature [" + << heatSignature << "] could not be found\n"; + return; + } + _ecm.CreateComponent(_entity, components::SourceFilePath(path)); + + // see if the user defined a custom temperature range + // (default to ambient temperature if possible) + double min = 250.0; + double max = 300.0; + if (auto worldEntity = _ecm.EntityByComponents(components::World())) + { + if (auto atmosphere = + _ecm.Component(worldEntity)) + { + auto atmosphericTemp = atmosphere->Data().Temperature().Kelvin(); + min = atmosphericTemp; + max = atmosphericTemp; + } + } + if (_sdf->HasElement(minTempTag)) + min = _sdf->Get(minTempTag); + if (_sdf->HasElement(maxTempTag)) + max = _sdf->Get(maxTempTag); + // make sure that min is actually less than max + if (min > max) + { + auto temporary = max; + max = min; + min = temporary; + } + igndbg << "Thermal plugin, heat signature: using a minimum temperature of " + << min << " kelvin, and a max temperature of " << max << " kelvin.\n"; + + components::TemperatureRangeInfo rangeInfo; + rangeInfo.min = min; + rangeInfo.max = max; + _ecm.CreateComponent(_entity, components::TemperatureRange(rangeInfo)); + } + else if (_sdf->HasElement(temperatureTag)) + { + double temperature = _sdf->Get(temperatureTag); + _ecm.CreateComponent(_entity, components::Temperature(temperature)); + } + else { - ignerr << "Fail to load thermal system: is not specified" - << std::endl; - return; + ignerr << "Failed to load thermal system. " + << "Neither <" << temperatureTag << "> or <" << heatSignatureTag + << "> were specified.\n"; } - double temperature = _sdf->Get("temperature"); - _ecm.CreateComponent(_entity, components::Temperature(temperature)); } diff --git a/src/systems/thermal/ThermalSensor.cc b/src/systems/thermal/ThermalSensor.cc new file mode 100644 index 0000000000..72e5f60a45 --- /dev/null +++ b/src/systems/thermal/ThermalSensor.cc @@ -0,0 +1,93 @@ +/* + * 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 "ThermalSensor.hh" + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private Thermal sensor data class. +class ignition::gazebo::systems::ThermalSensorPrivate +{ +}; + +////////////////////////////////////////////////// +ThermalSensor::ThermalSensor() : System(), + dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +ThermalSensor::~ThermalSensor() = default; + +////////////////////////////////////////////////// +void ThermalSensor::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager & /*_eventMgr*/) +{ + const std::string resolutionTag = "resolution"; + const std::string minTempTag = "min_temp"; + const std::string maxTempTag = "max_temp"; + + auto sensorComp = _ecm.Component(_entity); + if (sensorComp == nullptr) + { + ignerr << "The thermal sensor system can only be used to configure " + << "parameters of thermal camera sensor " << std::endl; + return; + } + + if (_sdf->HasElement(resolutionTag)) + { + double resolution = _sdf->Get(resolutionTag); + _ecm.CreateComponent(_entity, + components::TemperatureLinearResolution(resolution)); + } + + if (_sdf->HasElement(minTempTag) || _sdf->HasElement(maxTempTag)) + { + double min = 0.0; + double max = std::numeric_limits::max(); + min = _sdf->Get(minTempTag, min).first; + max = _sdf->Get(maxTempTag, max).first; + components::TemperatureRangeInfo rangeInfo; + rangeInfo.min = min; + rangeInfo.max = max; + _ecm.CreateComponent(_entity, components::TemperatureRange(rangeInfo)); + } +} + +IGNITION_ADD_PLUGIN(ThermalSensor, System, + ThermalSensor::ISystemConfigure +) + +IGNITION_ADD_PLUGIN_ALIAS(ThermalSensor, + "ignition::gazebo::systems::ThermalSensor") diff --git a/src/systems/thermal/ThermalSensor.hh b/src/systems/thermal/ThermalSensor.hh new file mode 100644 index 0000000000..c0a27306d3 --- /dev/null +++ b/src/systems/thermal/ThermalSensor.hh @@ -0,0 +1,60 @@ +/* + * 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_THERMALSENSOR_HH_ +#define IGNITION_GAZEBO_SYSTEMS_THERMALSENSOR_HH_ + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ThermalSensorPrivate; + + /// \brief A thermal sensor plugin for configuring thermal sensor properties + class ThermalSensor: + public System, + public ISystemConfigure + { + /// \brief Constructor + public: explicit ThermalSensor(); + + /// \brief Destructor + public: ~ThermalSensor() override; + + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index b9367f33aa..ab1b53dc7e 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -20,7 +20,9 @@ #include #include #include +#include #include +#include #include #include @@ -28,8 +30,10 @@ #include +#include #include #include +#include #include #include @@ -37,12 +41,16 @@ #include "ignition/common/Profiler.hh" #include "ignition/gazebo/components/Light.hh" +#include "ignition/gazebo/components/LightCmd.hh" +#include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/PhysicsCmd.hh" #include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" @@ -123,6 +131,70 @@ class RemoveCommand : public UserCommandBase public: bool Execute() final; }; +/// \brief Command to modify a light entity from simulation. +class LightCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message identifying the entity to be edited. + /// \param[in] _iface Pointer to user commands interface. + public: LightCommand(msgs::Light *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief Light equality comparison function. + public: std::function + lightEql { [](const msgs::Light &_a, const msgs::Light &_b) + { + return + _a.type() == _b.type() && + _a.name() == _b.name() && + math::equal( + _a.diffuse().a(), _b.diffuse().a(), 1e-6f) && + math::equal( + _a.diffuse().r(), _b.diffuse().r(), 1e-6f) && + math::equal( + _a.diffuse().g(), _b.diffuse().g(), 1e-6f) && + math::equal( + _a.diffuse().b(), _b.diffuse().b(), 1e-6f) && + math::equal( + _a.specular().a(), _b.specular().a(), 1e-6f) && + math::equal( + _a.specular().r(), _b.specular().r(), 1e-6f) && + math::equal( + _a.specular().g(), _b.specular().g(), 1e-6f) && + math::equal( + _a.specular().b(), _b.specular().b(), 1e-6f) && + math::equal( + _a.range(), _b.range(), 1e-6f) && + math::equal( + _a.attenuation_linear(), + _b.attenuation_linear(), + 1e-6f) && + math::equal( + _a.attenuation_constant(), + _b.attenuation_constant(), + 1e-6f) && + math::equal( + _a.attenuation_quadratic(), + _b.attenuation_quadratic(), + 1e-6f) && + _a.cast_shadows() == _b.cast_shadows() && + math::equal( + _a.direction().x(), _b.direction().x(), 1e-6) && + math::equal( + _a.direction().y(), _b.direction().y(), 1e-6) && + math::equal( + _a.direction().z(), _b.direction().z(), 1e-6) && + math::equal( + _a.spot_inner_angle(), _b.spot_inner_angle(), 1e-6f) && + math::equal( + _a.spot_outer_angle(), _b.spot_outer_angle(), 1e-6f) && + math::equal(_a.spot_falloff(), _b.spot_falloff(), 1e-6f); + }}; +}; + /// \brief Command to update an entity's pose transform. class PoseCommand : public UserCommandBase { @@ -146,6 +218,19 @@ class PoseCommand : public UserCommandBase math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); }}; }; + +/// \brief Command to modify the physics parameters of a simulation. +class PhysicsCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the new physics parameters. + /// \param[in] _iface Pointer to user commands interface. + public: PhysicsCommand(msgs::Physics *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; } } } @@ -178,6 +263,13 @@ class ignition::gazebo::systems::UserCommandsPrivate public: bool RemoveService(const msgs::Entity &_req, msgs::Boolean &_res); + /// \brief Callback for light service + /// \param[in] _req Request containing light update of an entity. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the light will be successfully updated. + /// \return True if successful. + public: bool LightService(const msgs::Light &_req, msgs::Boolean &_res); + /// \brief Callback for pose service /// \param[in] _req Request containing pose update of an entity. /// \param[in] _res True if message successfully received and queued. @@ -185,6 +277,13 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \return True if successful. public: bool PoseService(const msgs::Pose &_req, msgs::Boolean &_res); + /// \brief Callback for physics service + /// \param[in] _req Request containing updates to the physics parameters. + /// \param[in] _res True if message successfully received and queued. + /// It does not mean that the physics parameters will be successfully updated. + /// \return True if successful. + public: bool PhysicsService(const msgs::Physics &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -258,6 +357,21 @@ void UserCommands::Configure(const Entity &_entity, &UserCommandsPrivate::PoseService, this->dataPtr.get()); ignmsg << "Pose service on [" << poseService << "]" << std::endl; + + // Light service + std::string lightService{"/world/" + validWorldName + "/light_config"}; + this->dataPtr->node.Advertise(lightService, + &UserCommandsPrivate::LightService, this->dataPtr.get()); + + ignmsg << "Light configuration service on [" << lightService << "]" + << std::endl; + + // Physics service + std::string physicsService{"/world/" + validWorldName + "/set_physics"}; + this->dataPtr->node.Advertise(physicsService, + &UserCommandsPrivate::PhysicsService, this->dataPtr.get()); + + ignmsg << "Physics service on [" << physicsService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -350,6 +464,25 @@ bool UserCommandsPrivate::RemoveService(const msgs::Entity &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::LightService(const msgs::Light &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, msgs::Boolean &_res) @@ -369,6 +502,24 @@ bool UserCommandsPrivate::PoseService(const msgs::Pose &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req, + msgs::Boolean &_res) +{ + // Create command and push it to queue + auto msg = _req.New(); + msg->CopyFrom(_req); + auto cmd = std::make_unique(msg, this->iface); + // Push to pending + { + std::lock_guard lock(this->pendingMutex); + this->pendingCmds.push_back(std::move(cmd)); + } + + _res.set_data(true); + return true; +} + ////////////////////////////////////////////////// UserCommandBase::UserCommandBase(google::protobuf::Message *_msg, std::shared_ptr &_iface) @@ -657,6 +808,93 @@ bool RemoveCommand::Execute() return true; } + +////////////////////////////////////////////////// +LightCommand::LightCommand(msgs::Light *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool LightCommand::Execute() +{ + auto lightMsg = dynamic_cast(this->msg); + if (nullptr == lightMsg) + { + ignerr << "Internal error, null light message" << std::endl; + return false; + } + + Entity lightEntity{kNullEntity}; + + if (lightMsg->id() != kNullEntity) + { + lightEntity = lightMsg->id(); + } + else if (!lightMsg->name().empty()) + { + if (lightMsg->parent_id() != kNullEntity) + { + lightEntity = this->iface->ecm->EntityByComponents( + components::Name(lightMsg->name()), + components::ParentEntity(lightMsg->parent_id())); + } + else + { + lightEntity = this->iface->ecm->EntityByComponents( + components::Name(lightMsg->name())); + } + } + if (kNullEntity == lightEntity) + { + ignerr << "Failed to find light with name [" << lightMsg->name() + << "], ID [" << lightMsg->id() << "] and parent ID [" + << lightMsg->parent_id() << "]." << std::endl; + return false; + } + + if (!lightEntity) + { + ignmsg << "Failed to find light entity named [" << lightMsg->name() + << "]." << std::endl; + return false; + } + + auto lightPose = this->iface->ecm->Component(lightEntity); + if (nullptr == lightPose) + lightEntity = kNullEntity; + + if (!lightEntity) + { + ignmsg << "Pose component not available" << std::endl; + return false; + } + + if (lightMsg->has_pose()) + { + lightPose->Data().Pos() = msgs::Convert(lightMsg->pose()).Pos(); + } + + auto lightCmdComp = + this->iface->ecm->Component(lightEntity); + if (!lightCmdComp) + { + this->iface->ecm->CreateComponent( + lightEntity, components::LightCmd(*lightMsg)); + } + else + { + auto state = lightCmdComp->SetData(*lightMsg, this->lightEql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + this->iface->ecm->SetChanged(lightEntity, components::LightCmd::typeId, + state); + } + + return true; +} + ////////////////////////////////////////////////// PoseCommand::PoseCommand(msgs::Pose *_msg, std::shared_ptr &_iface) @@ -715,6 +953,39 @@ bool PoseCommand::Execute() return true; } +////////////////////////////////////////////////// +PhysicsCommand::PhysicsCommand(msgs::Physics *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool PhysicsCommand::Execute() +{ + auto physicsMsg = dynamic_cast(this->msg); + if (nullptr == physicsMsg) + { + ignerr << "Internal error, null physics message" << std::endl; + return false; + } + + auto worldEntity = this->iface->ecm->EntityByComponents(components::World()); + if (worldEntity == kNullEntity) + { + ignmsg << "Failed to find world entity" << std::endl; + return false; + } + + if (!this->iface->ecm->EntityHasComponentType(worldEntity, + components::PhysicsCmd().TypeId())) + { + this->iface->ecm->CreateComponent(worldEntity, + components::PhysicsCmd(*physicsMsg)); + } + + return true; +} IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, diff --git a/test/helpers/EnvTestFixture.hh b/test/helpers/EnvTestFixture.hh new file mode 100644 index 0000000000..984156ae78 --- /dev/null +++ b/test/helpers/EnvTestFixture.hh @@ -0,0 +1,37 @@ +/* + * Copyright (C) 2020 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_TEST_HELPERS_ENVTESTFIXTURE_HH_ +#define IGNITION_GAZEBO_TEST_HELPERS_ENVTESTFIXTURE_HH_ + +#include + +#include +#include +#include "ignition/gazebo/test_config.hh" + +class ServerFixture : public ::testing::TestWithParam +{ + protected: void SetUp() override + { + // Augment the system plugin path. In SetUp to avoid test order issues. + ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + + ignition::common::Console::SetVerbosity(4); + } +}; +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index b3c33508fc..7250621ca8 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -7,6 +7,7 @@ set(tests battery_plugin.cc breadcrumbs.cc buoyancy.cc + collada_world_exporter.cc components.cc contact_system.cc detachable_joint.cc @@ -16,10 +17,12 @@ set(tests events.cc examples_build.cc follow_actor_system.cc + fuel_cached_server.cc imu_system.cc joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc + joint_trajectory_controller_system.cc kinetic_energy_monitor_system.cc lift_drag_system.cc level_manager.cc @@ -30,7 +33,9 @@ set(tests magnetometer_system.cc model.cc multicopter.cc + multiple_servers.cc network_handshake.cc + particle_emitter.cc performer_detector.cc physics_system.cc play_pause.cc @@ -39,7 +44,6 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc - thermal_system.cc touch_plugin.cc triggered_publisher.cc user_commands.cc @@ -57,6 +61,8 @@ set(tests_needing_display gpu_lidar.cc rgbd_camera.cc sensors_system.cc + thermal_system.cc + thermal_sensor_system.cc ) # Add systems that need a valid display here. diff --git a/test/integration/collada_world_exporter.cc b/test/integration/collada_world_exporter.cc new file mode 100644 index 0000000000..ae677413c0 --- /dev/null +++ b/test/integration/collada_world_exporter.cc @@ -0,0 +1,83 @@ +/* + * 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" + +#include "helpers/UniqueTestDirectoryEnv.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +class ColladaWorldExporterFixture : public ::testing::Test +{ + public: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } + + public: void LoadWorld(const std::string &_path) + { + ServerConfig serverConfig; + serverConfig.SetResourceCache(test::UniqueTestDirectoryEnv::Path()); + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, _path)); + + std::cout << "Loading: " << serverConfig.SdfFile() << std::endl; + this->server = std::make_unique(serverConfig); + EXPECT_FALSE(server->Running()); + } + + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +TEST_F(ColladaWorldExporterFixture, ExportWorld) +{ + this->LoadWorld(common::joinPaths("test", "worlds", + "collada_world_exporter.sdf")); + + // Cleanup + common::removeAll("./collada_world_exporter_box_test"); + + // The export directory shouldn't exist. + EXPECT_FALSE(common::exists("./collada_world_exporter_box_test")); + + // Run one iteration which should export the world. + server->Run(true, 1, false); + + // The export directory should now exist. + EXPECT_TRUE(common::exists("./collada_world_exporter_box_test")); + + // Cleanup + common::removeAll("./collada_world_exporter_box_test"); +} + +///////////////////////////////////////////////// +/// Main +int main(int _argc, char **_argv) +{ + ::testing::InitGoogleTest(&_argc, _argv); + ::testing::AddGlobalTestEnvironment( + new test::UniqueTestDirectoryEnv("save_world_test_cache")); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/components.cc b/test/integration/components.cc index 41524836a1..4771967d62 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -17,6 +17,8 @@ #include +#include + #include #include @@ -62,6 +64,7 @@ #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Performer.hh" #include "ignition/gazebo/components/PerformerAffinity.hh" #include "ignition/gazebo/components/PerformerLevels.hh" @@ -71,6 +74,7 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" #include "ignition/gazebo/components/ThreadPitch.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -1377,6 +1381,45 @@ TEST_F(ComponentsTest, Static) EXPECT_FALSE(comp11 != comp12); } +///////////////////////////////////////////////// +TEST_F(ComponentsTest, TemperatureRange) +{ + // TODO(adlarkin) make sure min can't be >= max? + components::TemperatureRangeInfo range1; + range1.min = math::Temperature(125.0); + range1.max = math::Temperature(300.0); + + components::TemperatureRangeInfo range2; + range2.min = math::Temperature(140.0); + range2.max = math::Temperature(200.0); + + components::TemperatureRangeInfo range3; + range3.min = math::Temperature(125.0); + range3.max = math::Temperature(300.0); + + // Create components + auto comp1 = components::TemperatureRange(range1); + auto comp2 = components::TemperatureRange(range2); + auto comp3 = components::TemperatureRange(range3); + + // Equality operators + EXPECT_EQ(comp1, comp3); + EXPECT_NE(comp1, comp2); + EXPECT_NE(comp2, comp3); + EXPECT_FALSE(comp1 == comp2); + EXPECT_TRUE(comp1 != comp2); + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("125 300", ostr.str()); + + std::istringstream istr(ostr.str()); + components::TemperatureRange comp4; + comp4.Deserialize(istr); + EXPECT_EQ(comp1, comp4); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, ThreadPitch) { @@ -1467,3 +1510,95 @@ TEST_F(ComponentsTest, Scene) EXPECT_FALSE(comp3.Data().Grid()); EXPECT_TRUE(comp3.Data().OriginVisual()); } + +////////////////////////////////////////////////// +TEST_F(ComponentsTest, ParticleEmitter) +{ + msgs::ParticleEmitter emitter1; + emitter1.set_name("emitter1"); + emitter1.set_id(0); + emitter1.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); + emitter1.mutable_size()->set_x(1); + emitter1.mutable_size()->set_y(2); + emitter1.mutable_size()->set_z(3); + emitter1.set_rate(4.0); + emitter1.set_duration(5.0); + emitter1.set_emitting(false); + emitter1.mutable_particle_size()->set_x(0.1); + emitter1.mutable_particle_size()->set_y(0.2); + emitter1.mutable_particle_size()->set_z(0.3); + emitter1.set_lifetime(6.0); + emitter1.set_min_velocity(7.0); + emitter1.set_max_velocity(8.0); + emitter1.mutable_color_start()->set_r(1.0); + emitter1.mutable_color_start()->set_g(0); + emitter1.mutable_color_start()->set_b(0); + emitter1.mutable_color_start()->set_a(0); + emitter1.mutable_color_end()->set_r(1.0); + emitter1.mutable_color_end()->set_g(1.0); + emitter1.mutable_color_end()->set_b(1.0); + emitter1.mutable_color_end()->set_a(0); + emitter1.set_scale_rate(9.0); + emitter1.set_color_range_image("path_to_texture"); + + msgs::ParticleEmitter emitter2; + emitter2.set_name("emitter2"); + emitter2.set_id(1); + emitter2.set_type(ignition::msgs::ParticleEmitter_EmitterType_BOX); + emitter2.mutable_size()->set_x(1); + emitter2.mutable_size()->set_y(2); + emitter2.mutable_size()->set_z(3); + emitter2.set_rate(4.0); + emitter2.set_duration(5.0); + emitter2.set_emitting(false); + emitter2.mutable_particle_size()->set_x(0.1); + emitter2.mutable_particle_size()->set_y(0.2); + emitter2.mutable_particle_size()->set_z(0.3); + emitter2.set_lifetime(6.0); + emitter2.set_min_velocity(7.0); + emitter2.set_max_velocity(8.0); + emitter2.mutable_color_start()->set_r(1.0); + emitter2.mutable_color_start()->set_g(0); + emitter2.mutable_color_start()->set_b(0); + emitter2.mutable_color_start()->set_a(0); + emitter2.mutable_color_end()->set_r(1.0); + emitter2.mutable_color_end()->set_g(1.0); + emitter2.mutable_color_end()->set_b(1.0); + emitter2.mutable_color_end()->set_a(0); + emitter2.set_scale_rate(9.0); + emitter2.set_color_range_image("path_to_texture"); + + // Create components. + auto comp1 = components::ParticleEmitter(emitter1); + auto comp2 = components::ParticleEmitter(emitter2); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::ParticleEmitter comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1.Data().id(), comp3.Data().id()); +} + +////////////////////////////////////////////////// +TEST_F(ComponentsTest, ParticleEmitterCmd) +{ + msgs::ParticleEmitter emitter1; + emitter1.set_name("emitter1"); + emitter1.set_emitting(true); + + // Create components. + auto comp1 = components::ParticleEmitterCmd(emitter1); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::ParticleEmitter comp3; + comp3.Deserialize(istr); + EXPECT_EQ(comp1.Data().emitting(), comp3.Data().emitting()); + EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); +} diff --git a/test/integration/diff_drive_system.cc b/test/integration/diff_drive_system.cc index 372959628a..f8d95f64fa 100644 --- a/test/integration/diff_drive_system.cc +++ b/test/integration/diff_drive_system.cc @@ -359,7 +359,8 @@ TEST_P(DiffDriveTest, OdomFrameId) int sleep = 0; int maxSleep = 30; - for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) + // cppcheck-suppress knownConditionTrueFalse + for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) // NOLINT { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } @@ -405,10 +406,195 @@ TEST_P(DiffDriveTest, OdomCustomFrameId) auto pub = node.Advertise("/model/vehicle/cmd_vel"); node.Subscribe("/model/vehicle/odometry", odomCb); + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + 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(DiffDriveTest, Pose_VFrameId) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), + "frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(), + "vehicle/odom"); + + EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(), + "child_frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(), + "vehicle/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/tf", pose_VCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + 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(DiffDriveTest, Pose_VCustomFrameId) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive_custom_frame_id.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function Pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), + "frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(0).value().Get(0).c_str(), + "odom"); + + EXPECT_STREQ(_msg.pose(0).header().data(1).key().c_str(), + "child_frame_id"); + EXPECT_STREQ(_msg.pose(0).header().data(1).value().Get(0).c_str(), + "base_footprint"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/model/vehicle/tf", Pose_VCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + + 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(DiffDriveTest, Pose_VCustomTfTopic) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/diff_drive_custom_tf_topic.sdf"); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + unsigned int odomPosesCount = 0; + std::function pose_VCb = + [&odomPosesCount](const msgs::Pose_V &_msg) + { + ASSERT_TRUE(_msg.pose(0).has_header()); + ASSERT_TRUE(_msg.pose(0).header().has_stamp()); + + ASSERT_GT(_msg.pose(0).header().data_size(), 1); + + EXPECT_STREQ(_msg.pose(0).header().data(0).key().c_str(), "frame_id"); + EXPECT_STREQ( + _msg.pose(0).header().data(0).value().Get(0).c_str(), + "vehicle/odom"); + + EXPECT_STREQ( + _msg.pose(0).header().data(1).key().c_str(), "child_frame_id"); + EXPECT_STREQ( + _msg.pose(0).header().data(1).value().Get(0).c_str(), + "vehicle/chassis"); + + odomPosesCount++; + }; + + transport::Node node; + auto pub = node.Advertise("/model/vehicle/cmd_vel"); + node.Subscribe("/tf_foo", pose_VCb); + + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0.0, 0, 0.2)); + + pub.Publish(msg); + server.Run(true, 100, false); int sleep = 0; int maxSleep = 30; + // cppcheck-suppress knownConditionTrueFalse for (; odomPosesCount < 5 && sleep < maxSleep; ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/test/integration/fuel_cached_server.cc b/test/integration/fuel_cached_server.cc new file mode 100644 index 0000000000..a0182cc5d2 --- /dev/null +++ b/test/integration/fuel_cached_server.cc @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2018 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/ServerConfig.hh" + +#include "../test/helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace ignition::gazebo; +using namespace std::chrono_literals; + +///////////////////////////////////////////////// +TEST_P(ServerFixture, CachedFuelWorld) +{ + auto cachedWorldPath = + common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds"); + common::setenv("IGN_FUEL_CACHE_PATH", cachedWorldPath.c_str()); + + ServerConfig serverConfig; + auto fuelWorldURL = + "https://fuel.ignitionrobotics.org/1.0/OpenRobotics/worlds/Test%20world"; + EXPECT_TRUE(serverConfig.SetSdfFile(fuelWorldURL)); + + EXPECT_EQ(fuelWorldURL, serverConfig.SdfFile()); + EXPECT_TRUE(serverConfig.SdfString().empty()); + + // Check that world was loaded + auto server = Server(serverConfig); + EXPECT_NE(std::nullopt, server.Running(0)); + EXPECT_FALSE(*server.Running(0)); + + server.Run(true /*blocking*/, 1, false/*paused*/); + + EXPECT_NE(std::nullopt, server.Running(0)); + EXPECT_FALSE(*server.Running(0)); +} + +// Run multiple times. We want to make sure that static globals don't cause +// problems. +INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); diff --git a/test/integration/joint_trajectory_controller_system.cc b/test/integration/joint_trajectory_controller_system.cc new file mode 100644 index 0000000000..7e8bc21a09 --- /dev/null +++ b/test/integration/joint_trajectory_controller_system.cc @@ -0,0 +1,402 @@ +/* + * 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 "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/Name.hh" + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +#define TOL 1e-4 + +using namespace ignition; +using namespace gazebo; + +/// \brief Test fixture for JointTrajectoryController system +class JointTrajectoryControllerTestFixture : public ::testing::Test +{ + // Documentation inherited +protected: + void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + ignition::common::setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str()); + } +}; + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts position-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerPositionControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_position_control_joint1", + "RR_position_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = + "/model/RR_position_control/joint_trajectory"; + const std::string feedbackTopic = + "/model/RR_position_control/joint_trajectory_feedback"; + + // Define initial joint positions + const std::array initialPositions = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryPositions; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(666000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({-0.7854, 0.7854}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(333000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({1.5708, -0.7854}); + // Point3 + timeFromStart.set_sec(2); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryPositions.push_back({3.1416, -1.5708}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentPositions[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointPosition component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current position of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointPositionComponent = + _ecm.Component(joint); + if (nullptr != jointPositionComponent) + { + currentPositions[i] = jointPositionComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial position is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentPositions[i], initialPositions[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target positions to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_positions(trajectoryPositions[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryPositions.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryPositions.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target position of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryPositions.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target position + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentPositions[j], trajectoryPositions[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} + +///////////////////////////////////////////////// +// Tests that JointTrajectoryController accepts velocity-controlled joint +// trajectory +TEST_F(JointTrajectoryControllerTestFixture, + JointTrajectoryControllerVelocityControl) +{ + using namespace std::chrono_literals; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_trajectory_controller.sdf"; + + // Define joints of the model to investigate + const size_t kNumberOfJoints = 2; + const std::string jointNames[kNumberOfJoints] = {"RR_velocity_control_joint1", + "RR_velocity_control_joint2"}; + + // Define names of Ignition Transport topics + const std::string trajectoryTopic = "/test_custom_topic/velocity_control"; + const std::string feedbackTopic = + "/test_custom_topic/velocity_control_feedback"; + + // Define initial joint velocities + const std::array initialVelocities = {0.0, 0.0}; + + // Define a trajectory to follow + msgs::Duration timeFromStart; + std::vector trajectoryTimes; + std::vector> trajectoryVelocities; + // Point1 + timeFromStart.set_sec(0); + timeFromStart.set_nsec(500000000); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({0.5, 0.5}); + // Point2 + timeFromStart.set_sec(1); + timeFromStart.set_nsec(0); + trajectoryTimes.push_back(timeFromStart); + trajectoryVelocities.push_back({-1.0, 1.0}); + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Setup test system + test::Relay testSystem; + double currentVelocities[kNumberOfJoints]; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + // Create a JointVelocity component for each joint if it doesn't exist + for (const auto &jointName : jointNames) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointName)); + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointVelocity()); + } + } + }); + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + // Get the current velocity of each joint + for (std::size_t i = 0; i < kNumberOfJoints; ++i) + { + const auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name( + jointNames[i])); + const auto jointVelocityComponent = + _ecm.Component(joint); + if (nullptr != jointVelocityComponent) + { + currentVelocities[i] = jointVelocityComponent->Data()[0]; + } + } + }); + server.AddSystem(testSystem.systemPtr); + + // Step few iterations and assert that the initial velocity is kept + const std::size_t initIters = 10; + server.Run(true, initIters, false); + for (size_t i = 0; i < kNumberOfJoints; ++i) + { + EXPECT_NEAR(currentVelocities[i], initialVelocities[i], TOL); + } + + // Create new JointTrajectory message based on the defined trajectory + ignition::msgs::JointTrajectory msg; + for (const auto &jointName : jointNames) + { + msg.add_joint_names(jointName); + } + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + ignition::msgs::JointTrajectoryPoint point; + + // Set the temporal information for the point + auto time = point.mutable_time_from_start(); + time->set_sec(trajectoryTimes[i].sec()); + time->set_nsec(trajectoryTimes[i].nsec()); + + // Add target velocities to the point + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + point.add_velocities(trajectoryVelocities[i][j]); + } + + // Add point to the trajectory + msg.add_points(); + msg.mutable_points(i)->CopyFrom(point); + } + + // Verify that feedback is strictly increasing and reaches value of 1.0 + size_t count = 0; + std::function feedbackCallback = + [&](const ignition::msgs::Float &_msg) { + count++; + if (trajectoryVelocities.size() == count) + { + EXPECT_FLOAT_EQ(_msg.data(), 1.0f); + } + else + { + EXPECT_FLOAT_EQ(_msg.data(), static_cast(count) / + static_cast(trajectoryVelocities.size())); + } + }; + transport::Node node; + node.Subscribe(feedbackTopic, feedbackCallback); + + // Publish joint trajectory + auto pub = node.Advertise(trajectoryTopic); + pub.Publish(msg); + + // Wait for message to be published + std::this_thread::sleep_for(100ms); + + // Run the simulation while asserting the target velocity of all joints at + // each trajectory point + auto previousIterFromStart = 0; + for (size_t i = 0; i < trajectoryVelocities.size(); ++i) + { + // Number of iters required to reach time_from_start of the current + // point (1ms step size) + auto iterFromStart = trajectoryTimes[i].sec() * 1000 + + trajectoryTimes[i].nsec() / 1000000; + auto neededIters = iterFromStart - previousIterFromStart; + + // Run the simulation + server.Run(true, neededIters, false); + + // Assert that each joint reached its target velocity + for (size_t j = 0; j < kNumberOfJoints; ++j) + { + EXPECT_NEAR(currentVelocities[j], trajectoryVelocities[i][j], TOL); + } + + // Keep track of how many iterations have already passed + previousIterFromStart = iterFromStart; + } +} diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index a9bc0ea255..df16897a4e 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -166,7 +166,6 @@ class LogSystemTest : public ::testing::Test common::removeAll(this->logsDir); } common::createDirectories(this->logsDir); - common::createDirectories(this->logPlaybackDir); } // Append extension to the end of a path, removing the separator at @@ -267,10 +266,6 @@ class LogSystemTest : public ::testing::Test /// \brief Path to recorded log file public: std::string logDir = common::joinPaths(logsDir, "test_logs_record"); - - /// \brief Path to log file for playback - public: std::string logPlaybackDir = - common::joinPaths(this->logsDir, "test_logs_playback"); }; ///////////////////////////////////////////////// @@ -342,16 +337,16 @@ TEST_F(LogSystemTest, LogDefaults) EXPECT_TRUE(ignition::common::setenv(IGN_HOMEDIR, homeFake.c_str())); // Test case 1: - // No path specified, on both command line and SDF. This does not go through + // No path specified on command line. This does not go through // ign.cc, so ignLogDirectory() is not initialized (empty string). Recording // should not take place. { - // Change log path in SDF to empty + // Load SDF sdf::Root recordSdfRoot; - this->ChangeLogPath(recordSdfRoot, recordSdfPath, "LogRecord", - " "); + EXPECT_EQ(recordSdfRoot.Load(recordSdfPath).size(), 0lu); + EXPECT_GT(recordSdfRoot.WorldCount(), 0lu); - // Pass changed SDF to server + // Pass SDF to server ServerConfig recordServerConfig; recordServerConfig.SetSdfString(recordSdfRoot.Element()->ToString("")); @@ -744,25 +739,17 @@ TEST_F(LogSystemTest, RecordAndPlayback) auto logFile = common::joinPaths(this->logDir, "state.tlog"); EXPECT_TRUE(common::exists(logFile)); - // move the log file to the playback directory - auto logPlaybackFile = common::joinPaths(this->logPlaybackDir, "state.tlog"); + // Path to log file for playback + std::string logPlaybackDir = + common::joinPaths(this->logsDir, "test_logs_playback"); + common::createDirectories(logPlaybackDir); + + // Move the log file to the playback directory + auto logPlaybackFile = common::joinPaths(logPlaybackDir, "state.tlog"); common::moveFile(logFile, logPlaybackFile); EXPECT_TRUE(common::exists(logPlaybackFile)); - // World file to load - const auto playSdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "log_playback.sdf"); - - // Change log path in world SDF to build directory - sdf::Root playSdfRoot; - this->ChangeLogPath(playSdfRoot, playSdfPath, "LogPlayback", - this->logPlaybackDir); - ASSERT_EQ(1u, playSdfRoot.WorldCount()); - - const auto sdfWorld = playSdfRoot.WorldByIndex(0); - EXPECT_EQ("default", sdfWorld->Name()); - - // Load log file recorded above + // Load log file recorded above for validation transport::log::Log log; log.Open(logPlaybackFile); @@ -795,10 +782,6 @@ TEST_F(LogSystemTest, RecordAndPlayback) EXPECT_EQ(32, stateMsg.entities_size()); EXPECT_EQ(batch.end(), ++recordedIter); - // Pass changed SDF to server - ServerConfig playServerConfig; - playServerConfig.SetSdfString(playSdfRoot.Element()->ToString("")); - // Keep track of total number of pose comparisons int nTotal{0}; @@ -819,6 +802,10 @@ TEST_F(LogSystemTest, RecordAndPlayback) EXPECT_EQ(0, recordedMsg.header().stamp().sec()); EXPECT_EQ(1000000, recordedMsg.header().stamp().nsec()); + // Playback config + ServerConfig playServerConfig; + playServerConfig.SetLogPlaybackPath(logPlaybackDir); + // Start server Server playServer(playServerConfig); @@ -1027,7 +1014,7 @@ TEST_F(LogSystemTest, LogOverwrite) // Create temp directory to store log this->CreateLogsDir(); #ifndef __APPLE__ - EXPECT_EQ(1, entryCount(this->logsDir)); + EXPECT_EQ(0, entryCount(this->logsDir)); EXPECT_EQ(0, entryCount(this->logDir)); #endif @@ -1063,7 +1050,7 @@ TEST_F(LogSystemTest, LogOverwrite) #ifndef __APPLE__ // Log files were created - EXPECT_EQ(2, entryCount(this->logsDir)); + EXPECT_EQ(1, entryCount(this->logsDir)); EXPECT_EQ(2, entryCount(this->logDir)); std::filesystem::path tlogStdPath = tlogPath; auto tlogPrevTime = std::filesystem::last_write_time(tlogStdPath); @@ -1089,7 +1076,7 @@ TEST_F(LogSystemTest, LogOverwrite) #ifndef __APPLE__ // No new files were created - EXPECT_EQ(2, entryCount(this->logsDir)); + EXPECT_EQ(1, entryCount(this->logsDir)); EXPECT_EQ(2, entryCount(this->logDir)); // Test timestamp is newer @@ -1117,7 +1104,7 @@ TEST_F(LogSystemTest, LogOverwrite) EXPECT_TRUE(common::exists(clogPath)); // No new files were created - EXPECT_EQ(2, entryCount(this->logsDir)); + EXPECT_EQ(1, entryCount(this->logsDir)); EXPECT_EQ(2, entryCount(this->logDir)); // Test timestamp is newer @@ -1159,7 +1146,7 @@ TEST_F(LogSystemTest, LogOverwrite) "server_console.log"))); // New files were created - EXPECT_EQ(3, entryCount(this->logsDir)); + EXPECT_EQ(2, entryCount(this->logsDir)); EXPECT_EQ(2, entryCount(this->logDir)); EXPECT_EQ(2, entryCount(this->logDir + "(1)")); @@ -1176,16 +1163,8 @@ TEST_F(LogSystemTest, LogControlLevels) auto logPath = common::joinPaths(PROJECT_SOURCE_PATH, "test", "media", "levels_log"); - const auto playSdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "log_playback.sdf"); - - // Change log path in world SDF to build directory - sdf::Root playSdfRoot; - this->ChangeLogPath(playSdfRoot, playSdfPath, "LogPlayback", - logPath); - ServerConfig config; - config.SetSdfString(playSdfRoot.Element()->ToString("")); + config.SetLogPlaybackPath(logPath); Server server(config); @@ -1391,29 +1370,25 @@ TEST_F(LogSystemTest, LogCompress) // Test compressed file exists EXPECT_TRUE(common::exists(customCmpPath)); + // Path to log file for playback + std::string logPlaybackDir = + common::joinPaths(this->logsDir, "test_logs_playback"); + common::createDirectories(logPlaybackDir); + // Move recorded file to playback directory // Prefix the zip by the name of the original recorded folder. Playback will // extract and assume subdirectory to take on the name of the zip file // without extension. - auto newCmpPath = common::joinPaths(this->logPlaybackDir, + auto newCmpPath = common::joinPaths(logPlaybackDir, common::basename(defaultCmpPath)); common::moveFile(customCmpPath, newCmpPath); EXPECT_TRUE(common::exists(newCmpPath)); // Play back compressed file { - // World with playback plugin - const auto playSdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "log_playback.sdf"); - - // Change log path in world SDF to build directory - sdf::Root playSdfRoot; - this->ChangeLogPath(playSdfRoot, playSdfPath, "LogPlayback", - newCmpPath); - // Pass changed SDF to server ServerConfig playServerConfig; - playServerConfig.SetSdfString(playSdfRoot.Element()->ToString("")); + playServerConfig.SetLogPlaybackPath(newCmpPath); // Run server Server playServer(playServerConfig); diff --git a/test/integration/logical_camera_system.cc b/test/integration/logical_camera_system.cc index 682356f0b5..6d68aaa995 100644 --- a/test/integration/logical_camera_system.cc +++ b/test/integration/logical_camera_system.cc @@ -191,5 +191,4 @@ TEST_F(LogicalCameraTest, LogicalCameraBox) ignition::math::Pose3d boxPoseCamera2Frame = boxPose - sensor2Pose; EXPECT_EQ(boxPoseCamera2Frame, ignition::msgs::Convert(img2.model(0).pose())); mutex.unlock(); - } diff --git a/test/integration/multiple_servers.cc b/test/integration/multiple_servers.cc new file mode 100644 index 0000000000..71f89ab663 --- /dev/null +++ b/test/integration/multiple_servers.cc @@ -0,0 +1,104 @@ +/* + * Copyright (C) 2018 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/ServerConfig.hh" + +#include "../test/helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace ignition::gazebo; +using namespace std::chrono_literals; + +///////////////////////////////////////////////// +TEST_P(ServerFixture, TwoServersNonBlocking) +{ + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfString(TestWorldSansPhysics::World()); + + gazebo::Server server1(serverConfig); + gazebo::Server server2(serverConfig); + EXPECT_FALSE(server1.Running()); + EXPECT_FALSE(*server1.Running(0)); + EXPECT_FALSE(server2.Running()); + EXPECT_FALSE(*server2.Running(0)); + EXPECT_EQ(0u, *server1.IterationCount()); + EXPECT_EQ(0u, *server2.IterationCount()); + + // Make the servers run fast. + server1.SetUpdatePeriod(1ns); + server2.SetUpdatePeriod(1ns); + + // Start non-blocking + const size_t iters1 = 9999; + EXPECT_TRUE(server1.Run(false, iters1, false)); + + // Expect that we can't start another instance. + EXPECT_FALSE(server1.Run(true, 10, false)); + + // It's okay to start another server + EXPECT_TRUE(server2.Run(false, 500, false)); + + while (*server1.IterationCount() < iters1 || *server2.IterationCount() < 500) + IGN_SLEEP_MS(100); + + EXPECT_EQ(iters1, *server1.IterationCount()); + EXPECT_EQ(500u, *server2.IterationCount()); + EXPECT_FALSE(server1.Running()); + EXPECT_FALSE(*server1.Running(0)); + EXPECT_FALSE(server2.Running()); + EXPECT_FALSE(*server2.Running(0)); +} + +///////////////////////////////////////////////// +TEST_P(ServerFixture, TwoServersMixedBlocking) +{ + ignition::gazebo::ServerConfig serverConfig; + serverConfig.SetSdfString(TestWorldSansPhysics::World()); + + gazebo::Server server1(serverConfig); + gazebo::Server server2(serverConfig); + EXPECT_FALSE(server1.Running()); + EXPECT_FALSE(*server1.Running(0)); + EXPECT_FALSE(server2.Running()); + EXPECT_FALSE(*server2.Running(0)); + EXPECT_EQ(0u, *server1.IterationCount()); + EXPECT_EQ(0u, *server2.IterationCount()); + + // Make the servers run fast. + server1.SetUpdatePeriod(1ns); + server2.SetUpdatePeriod(1ns); + + server1.Run(false, 10, false); + server2.Run(true, 1000, false); + + while (*server1.IterationCount() < 10) + IGN_SLEEP_MS(100); + + EXPECT_EQ(10u, *server1.IterationCount()); + EXPECT_EQ(1000u, *server2.IterationCount()); + EXPECT_FALSE(server1.Running()); + EXPECT_FALSE(*server1.Running(0)); + EXPECT_FALSE(server2.Running()); + EXPECT_FALSE(*server2.Running(0)); +} + +// Run multiple times. We want to make sure that static globals don't cause +// problems. +INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); diff --git a/test/integration/particle_emitter.cc b/test/integration/particle_emitter.cc new file mode 100644 index 0000000000..f2527b8357 --- /dev/null +++ b/test/integration/particle_emitter.cc @@ -0,0 +1,164 @@ +/* + * 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 ParticleEmitterTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + ignition::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(ParticleEmitterTest, SDFLoad) +{ + bool updateCustomChecked{false}; + bool updateDefaultChecked{false}; + + this->LoadWorld("test/worlds/particle_emitter.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()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().duration()); + EXPECT_TRUE(_emitter->Data().emitting()); + EXPECT_EQ(math::Vector3d(3, 3, 3), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(2.0, _emitter->Data().lifetime()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(10.0, _emitter->Data().min_velocity()); + EXPECT_DOUBLE_EQ(20.0, _emitter->Data().max_velocity()); + 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()); + + // color range image is empty because the emitter system + // will not be able to find a file that does not exist + // TODO(anyone) this should return "/path/to/dummy_image.png" + // and let rendering do the findFile instead + EXPECT_EQ(std::string(), + _emitter->Data().color_range_image()); + } + 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()); + EXPECT_DOUBLE_EQ(0.0, _emitter->Data().duration()); + EXPECT_FALSE(_emitter->Data().emitting()); + EXPECT_EQ(math::Vector3d(1, 1, 1), + msgs::Convert(_emitter->Data().particle_size())); + EXPECT_DOUBLE_EQ(5.0, _emitter->Data().lifetime()); + // TODO(anyone) add material check here + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().min_velocity()); + EXPECT_DOUBLE_EQ(1.0, _emitter->Data().max_velocity()); + 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()); + EXPECT_EQ("", _emitter->Data().color_range_image()); + } + + return true; + }); + }); + + this->server->AddSystem(testSystem.systemPtr); + this->server->Run(true, 1, false); + + EXPECT_TRUE(updateCustomChecked); + EXPECT_TRUE(updateDefaultChecked); +} diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 906fd980b9..ad564bf5ad 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -439,6 +439,7 @@ TEST_P(SceneBroadcasterTest, State) while (!received && sleep++ < maxSleep) IGN_SLEEP_MS(100); EXPECT_TRUE(received); + EXPECT_TRUE(node.Unsubscribe("/world/default/state")); // test async state request received = false; diff --git a/test/integration/thermal_sensor_system.cc b/test/integration/thermal_sensor_system.cc new file mode 100644 index 0000000000..be34ca4a8b --- /dev/null +++ b/test/integration/thermal_sensor_system.cc @@ -0,0 +1,220 @@ +/* + * 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 "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" + +using namespace ignition; +using namespace gazebo; + +/// \brief Test Thermal system +class ThermalSensorTest : public ::testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + common::Console::SetVerbosity(4); + setenv("IGN_GAZEBO_SYSTEM_PLUGIN_PATH", + (std::string(PROJECT_BINARY_PATH) + "/lib").c_str(), 1); + } +}; + + +std::mutex g_mutex; +std::vector g_imageMsgs; +unsigned char *g_image = nullptr; + +///////////////////////////////////////////////// +void thermalCb(const msgs::Image &_msg) +{ + std::lock_guard g_lock(g_mutex); + g_imageMsgs.push_back(_msg); + + unsigned int width = _msg.width(); + unsigned int height = _msg.height(); + unsigned int size = width * height * sizeof(unsigned char); + if (!g_image) + { + g_image = new unsigned char[size]; + } + memcpy(g_image, _msg.data().c_str(), size); +} + +///////////////////////////////////////////////// +TEST_F(ThermalSensorTest, + IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystemInvalidConfig)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal_invalid.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::map entityTemp; + std::string name; + sdf::Sensor sensorSdf; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::Temperature *_temp, + const components::Name *_name) -> bool + { + // store temperature data + entityTemp[_name->Data()] = _temp->Data(); + + // verify temperature data belongs to a visual + EXPECT_NE(nullptr, _ecm.Component(_id)); + + return true; + }); + }); + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + if (resolutionComp) + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + if (temperatureRangeComp) + { + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // subscribe to thermal topic + transport::Node node; + node.Subscribe("/thermal_camera_invalid/image", &thermalCb); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + unsigned int width = 320u; + unsigned int height = 240u; + EXPECT_EQ("thermal_camera_invalid", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(width, cameraSdf->ImageWidth()); + EXPECT_EQ(height, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + // the resolution, min and max are invalid range values. Ign-gazebo should + // print out warnings and use default values + EXPECT_DOUBLE_EQ(0.0, resolution); + EXPECT_DOUBLE_EQ(999.0, minTemp); + EXPECT_DOUBLE_EQ(-590.0, maxTemp); + + // verify temperature of heat source + const std::string sphereVisual = "sphere_visual"; + const std::string cylinderVisual = "cylinder_visual"; + EXPECT_EQ(2u, entityTemp.size()); + ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end()); + ASSERT_TRUE(entityTemp.find(cylinderVisual) != 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()); + + // Run server + server.Run(true, 35, false); + + // wait for image + bool received = false; + for (unsigned int i = 0; i < 20; ++i) + { + { + std::lock_guard lock(g_mutex); + received = !g_imageMsgs.empty(); + } + if (received) + break; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); + ASSERT_NE(nullptr, g_image); + + // check temperature in actual image output + { + std::lock_guard lock(g_mutex); + unsigned int leftIdx = height * 0.5 * width; + unsigned int rightIdx = leftIdx + width-1; + unsigned int defaultResolution = 3u; + unsigned int cylinderTemp = g_image[leftIdx] * defaultResolution; + unsigned int sphereTemp = g_image[rightIdx] * defaultResolution; + // default resolution, min, max values used so we should get correct + // temperature value + EXPECT_EQ(600u, sphereTemp); + // 8 bit 3 degree resolution camera - this value should be clamped + // in the image output to: + // 2^(bitDepth) - 1 * resolution = 2^8 - 1 * 3 = 765 + EXPECT_EQ(765u, cylinderTemp); + } + + g_imageMsgs.clear(); + delete [] g_image; + g_image = nullptr; +} diff --git a/test/integration/thermal_system.cc b/test/integration/thermal_system.cc index ae69c7fcbf..aad2f7dc30 100644 --- a/test/integration/thermal_system.cc +++ b/test/integration/thermal_system.cc @@ -16,13 +16,22 @@ */ #include + +#include +#include + #include +#include #include #include #include +#include #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" +#include "ignition/gazebo/components/TemperatureRange.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" @@ -30,8 +39,6 @@ #include "../helpers/Relay.hh" -#define tol 10e-4 - using namespace ignition; using namespace gazebo; @@ -48,7 +55,7 @@ class ThermalTest : public ::testing::Test }; ///////////////////////////////////////////////// -TEST_F(ThermalTest, TemperatureComponent) +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(TemperatureComponent)) { // Start server ServerConfig serverConfig; @@ -63,6 +70,9 @@ TEST_F(ThermalTest, TemperatureComponent) test::Relay testSystem; std::map entityTemp; + std::map + entityTempRange; + std::map heatSignatures; testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { @@ -77,20 +87,160 @@ TEST_F(ThermalTest, TemperatureComponent) // verify temperature data belongs to a visual EXPECT_NE(nullptr, _ecm.Component(_id)); + return true; + }); + + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::TemperatureRange *_tempRange, + const components::SourceFilePath *_heatSigURI, + const components::Name *_name) -> bool + { + // store temperature range data + entityTempRange[_name->Data()] = _tempRange->Data(); + + // store heat signature URI data + heatSignatures[_name->Data()] = _heatSigURI->Data(); + + // verify temperature range data belongs to a visual + EXPECT_NE(nullptr, _ecm.Component(_id)); + return true; }); }); server.AddSystem(testSystem.systemPtr); - // verify nothing in map at beginning + // verify nothing in the maps at beginning EXPECT_TRUE(entityTemp.empty()); + EXPECT_TRUE(entityTempRange.empty()); + EXPECT_TRUE(heatSignatures.empty()); // Run server server.Run(true, 1, false); + const std::string sphereVisual = "sphere_visual"; + const std::string cylinderVisual = "cylinder_visual"; + const std::string visual = "visual"; + const std::string heatSignatureCylinderVisual = + "heat_signature_cylinder_visual"; + const std::string heatSignatureSphereVisual = + "heat_signature_sphere_visual"; + const std::string heatSignatureSphereVisual2 = + "heat_signature_sphere_visual_2"; + const std::string heatSignatureTestResource = "duck.png"; + // verify temperature components are created and the values are correct - EXPECT_EQ(3u, entityTemp.size()); - EXPECT_DOUBLE_EQ(200.0, entityTemp["box_visual"].Kelvin()); - EXPECT_DOUBLE_EQ(600.0, entityTemp["sphere_visual"].Kelvin()); - EXPECT_DOUBLE_EQ(400.0, entityTemp["cylinder_visual"].Kelvin()); + EXPECT_EQ(2u, entityTemp.size()); + ASSERT_TRUE(entityTemp.find(sphereVisual) != entityTemp.end()); + ASSERT_TRUE(entityTemp.find(cylinderVisual) != entityTemp.end()); + EXPECT_DOUBLE_EQ(600.0, entityTemp[sphereVisual].Kelvin()); + EXPECT_DOUBLE_EQ(400.0, entityTemp[cylinderVisual].Kelvin()); + + EXPECT_EQ(4u, entityTempRange.size()); + ASSERT_TRUE(entityTempRange.find(visual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureCylinderVisual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureSphereVisual) != entityTempRange.end()); + ASSERT_TRUE(entityTempRange.find( + heatSignatureSphereVisual2) != entityTempRange.end()); + EXPECT_DOUBLE_EQ(310.0, entityTempRange[visual].min.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, entityTempRange[visual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureCylinderVisual].min.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureCylinderVisual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureSphereVisual].min.Kelvin()); + EXPECT_DOUBLE_EQ(500.0, + entityTempRange[heatSignatureSphereVisual].max.Kelvin()); + EXPECT_DOUBLE_EQ(310.0, + entityTempRange[heatSignatureSphereVisual2].min.Kelvin()); + EXPECT_DOUBLE_EQ(400.0, + entityTempRange[heatSignatureSphereVisual2].max.Kelvin()); + + EXPECT_EQ(4u, heatSignatures.size()); + ASSERT_TRUE(heatSignatures.find(visual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureCylinderVisual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureSphereVisual) != heatSignatures.end()); + ASSERT_TRUE(heatSignatures.find( + heatSignatureSphereVisual2) != heatSignatures.end()); + EXPECT_TRUE(heatSignatures[visual].find( + "RescueRandy_Thermal.png") != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureCylinderVisual].find( + heatSignatureTestResource) != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual].find( + heatSignatureTestResource) != std::string::npos); + EXPECT_TRUE(heatSignatures[heatSignatureSphereVisual2].find( + heatSignatureTestResource) != std::string::npos); +} + +///////////////////////////////////////////////// +TEST_F(ThermalTest, IGN_UTILS_TEST_DISABLED_ON_MAC(ThermalSensorSystem)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test/worlds/thermal.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that checks for thermal component + test::Relay testSystem; + + double resolution = 0; + double minTemp = std::numeric_limits::max(); + double maxTemp = 0.0; + std::string name; + sdf::Sensor sensorSdf; + testSystem.OnUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_id, + const components::ThermalCamera *_sensor, + const components::Name *_name) -> bool + { + // store temperature data + sensorSdf = _sensor->Data(); + name = _name->Data(); + + auto resolutionComp = + _ecm.Component( + _id); + EXPECT_NE(nullptr, resolutionComp); + resolution = resolutionComp->Data(); + + auto temperatureRangeComp = + _ecm.Component(_id); + EXPECT_NE(nullptr, temperatureRangeComp); + auto info = temperatureRangeComp->Data(); + minTemp = info.min.Kelvin(); + maxTemp = info.max.Kelvin(); + return true; + }); + + }); + server.AddSystem(testSystem.systemPtr); + + // Run server + server.Run(true, 1, false); + + // verify camera properties from sdf + EXPECT_EQ("thermal_camera_8bit", name); + const sdf::Camera *cameraSdf = sensorSdf.CameraSensor(); + ASSERT_NE(nullptr, cameraSdf); + EXPECT_EQ(320u, cameraSdf->ImageWidth()); + EXPECT_EQ(240u, cameraSdf->ImageHeight()); + EXPECT_EQ(sdf::PixelFormatType::L_INT8, cameraSdf->PixelFormat()); + + // verify camera properties set through plugin + EXPECT_DOUBLE_EQ(3.0, resolution); + EXPECT_DOUBLE_EQ(253.15, minTemp); + EXPECT_DOUBLE_EQ(673.15, maxTemp); } diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index c2709e2901..d9d398d154 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include #include @@ -28,7 +30,9 @@ #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/test_config.hh" @@ -691,3 +695,291 @@ TEST_F(UserCommandsTest, Pose) ASSERT_NE(nullptr, poseComp); EXPECT_NEAR(500.0, poseComp->Data().Pos().Y(), 0.2); } + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, Light) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = ignition::common::joinPaths( + std::string(PROJECT_SOURCE_PATH), "test", "worlds", "lights_render.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + msgs::Light req; + msgs::Boolean res; + transport::Node node; + bool result; + unsigned int timeout = 1000; + std::string service{"/world/lights_command/light_config"}; + + // Point light + auto pointLightEntity = ecm->EntityByComponents(components::Name("point")); + EXPECT_NE(kNullEntity, pointLightEntity); + + // Check point light entity has not been edited yet - Initial values + auto pointLightComp = ecm->Component(pointLightEntity); + ASSERT_NE(nullptr, pointLightComp); + EXPECT_EQ( + math::Pose3d(0, -1.5, 3, 0, 0, 0), pointLightComp->Data().RawPose()); + EXPECT_EQ(math::Color(1, 0, 0, 1), pointLightComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.1, 0.1, 0.1, 1), pointLightComp->Data().Specular()); + EXPECT_NEAR(4.0, pointLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR(0.5, pointLightComp->Data().LinearAttenuationFactor(), 0.1); + EXPECT_NEAR(0.2, pointLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR(0.01, pointLightComp->Data().QuadraticAttenuationFactor(), 0.1); + EXPECT_FALSE(pointLightComp->Data().CastShadows()); + + req.Clear(); + ignition::msgs::Set(req.mutable_diffuse(), + ignition::math::Color(0, 1, 1, 0)); + ignition::msgs::Set(req.mutable_specular(), + ignition::math::Color(0.2, 0.2, 0.2, 0.2)); + req.set_range(2.6); + req.set_name("point"); + req.set_type(ignition::msgs::Light::POINT); + req.set_attenuation_linear(0.7); + req.set_attenuation_constant(0.6); + req.set_attenuation_quadratic(0.001); + req.set_cast_shadows(true); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + server.Run(true, 100, false); + // Sleep for a small duration to allow Run thread to start + IGN_SLEEP_MS(10); + + // Check point light entity has been edited using the service + pointLightComp = ecm->Component(pointLightEntity); + ASSERT_NE(nullptr, pointLightComp); + + EXPECT_EQ(math::Color(0, 1, 1, 0), pointLightComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.2, 0.2, 0.2, 0.2), + pointLightComp->Data().Specular()); + EXPECT_NEAR(2.6, pointLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR(0.7, pointLightComp->Data().LinearAttenuationFactor(), 0.1); + EXPECT_NEAR(0.6, pointLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR(0.001, pointLightComp->Data().QuadraticAttenuationFactor(), 0.1); + EXPECT_TRUE(pointLightComp->Data().CastShadows()); + EXPECT_EQ(sdf::LightType::POINT, pointLightComp->Data().Type()); + + // Check directional light entity has not been edited yet - Initial values + auto directionalLightEntity = ecm->EntityByComponents( + components::Name("directional")); + EXPECT_NE(kNullEntity, directionalLightEntity); + + auto directionalLightComp = + ecm->Component(directionalLightEntity); + ASSERT_NE(nullptr, directionalLightComp); + + EXPECT_EQ( + math::Pose3d(0, 0, 10, 0, 0, 0), directionalLightComp->Data().RawPose()); + EXPECT_EQ( + math::Color(0.8, 0.8, 0.8, 1), directionalLightComp->Data().Diffuse()); + EXPECT_EQ( + math::Color(0.2, 0.2, 0.2, 1), directionalLightComp->Data().Specular()); + EXPECT_NEAR(100, directionalLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR( + 0.01, directionalLightComp->Data().LinearAttenuationFactor(), 0.01); + EXPECT_NEAR( + 0.9, directionalLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR( + 0.001, directionalLightComp->Data().QuadraticAttenuationFactor(), 0.001); + EXPECT_EQ( + math::Vector3d(0.5, 0.2, -0.9), directionalLightComp->Data().Direction()); + EXPECT_TRUE(directionalLightComp->Data().CastShadows()); + EXPECT_EQ(sdf::LightType::POINT, pointLightComp->Data().Type()); + + req.Clear(); + ignition::msgs::Set(req.mutable_diffuse(), + ignition::math::Color(0, 1, 1, 0)); + ignition::msgs::Set(req.mutable_specular(), + ignition::math::Color(0.3, 0.3, 0.3, 0.3)); + req.set_range(2.6); + req.set_name("directional"); + req.set_type(ignition::msgs::Light::DIRECTIONAL); + req.set_attenuation_linear(0.7); + req.set_attenuation_constant(0.6); + req.set_attenuation_quadratic(1); + req.set_cast_shadows(false); + ignition::msgs::Set(req.mutable_direction(), + ignition::math::Vector3d(1, 2, 3)); + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + server.Run(true, 100, false); + // Sleep for a small duration to allow Run thread to start + IGN_SLEEP_MS(10); + + // Check directional light entity has been edited using the service + directionalLightComp = + ecm->Component(directionalLightEntity); + ASSERT_NE(nullptr, directionalLightComp); + + EXPECT_EQ(math::Color(0, 1, 1, 0), directionalLightComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.3, 0.3, 0.3, 0.3), + directionalLightComp->Data().Specular()); + EXPECT_NEAR(2.6, directionalLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR( + 0.7, directionalLightComp->Data().LinearAttenuationFactor(), 0.1); + EXPECT_NEAR( + 0.6, directionalLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR( + 1, directionalLightComp->Data().QuadraticAttenuationFactor(), 0.1); + EXPECT_EQ(math::Vector3d(1, 2, 3), directionalLightComp->Data().Direction()); + EXPECT_FALSE(directionalLightComp->Data().CastShadows()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, + directionalLightComp->Data().Type()); + + // spot light + auto spotLightEntity = ecm->EntityByComponents( + components::Name("spot")); + EXPECT_NE(kNullEntity, spotLightEntity); + + // Check spot light entity has not been edited yet - Initial values + auto spotLightComp = + ecm->Component(spotLightEntity); + ASSERT_NE(nullptr, spotLightComp); + + EXPECT_EQ(math::Pose3d(0, 1.5, 3, 0, 0, 0), spotLightComp->Data().RawPose()); + EXPECT_EQ(math::Color(0, 1, 0, 1), spotLightComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.2, 0.2, 0.2, 1), spotLightComp->Data().Specular()); + EXPECT_NEAR(5, spotLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR(0.4, spotLightComp->Data().LinearAttenuationFactor(), 0.01); + EXPECT_NEAR(0.3, spotLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR( + 0.001, spotLightComp->Data().QuadraticAttenuationFactor(), 0.001); + EXPECT_EQ(math::Vector3d(0, 0, -1), spotLightComp->Data().Direction()); + EXPECT_FALSE(spotLightComp->Data().CastShadows()); + EXPECT_EQ(sdf::LightType::SPOT, spotLightComp->Data().Type()); + EXPECT_NEAR(0.1, spotLightComp->Data().SpotInnerAngle().Radian(), 0.1); + EXPECT_NEAR(0.5, spotLightComp->Data().SpotOuterAngle().Radian(), 0.1); + EXPECT_NEAR(0.8, spotLightComp->Data().SpotFalloff(), 0.1); + + req.Clear(); + ignition::msgs::Set(req.mutable_diffuse(), + ignition::math::Color(1, 0, 1, 0)); + ignition::msgs::Set(req.mutable_specular(), + ignition::math::Color(0.3, 0.3, 0.3, 0.3)); + req.set_range(2.6); + req.set_name("spot"); + req.set_type(ignition::msgs::Light::SPOT); + req.set_attenuation_linear(0.7); + req.set_attenuation_constant(0.6); + req.set_attenuation_quadratic(1); + req.set_cast_shadows(true); + ignition::msgs::Set(req.mutable_direction(), + ignition::math::Vector3d(1, 2, 3)); + req.set_spot_inner_angle(1.5); + req.set_spot_outer_angle(0.3); + req.set_spot_falloff(0.9); + + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + server.Run(true, 100, false); + // Sleep for a small duration to allow Run thread to start + IGN_SLEEP_MS(10); + + // Check spot light entity has been edited using the service + spotLightComp = ecm->Component(spotLightEntity); + ASSERT_NE(nullptr, spotLightComp); + + EXPECT_EQ(math::Color(1, 0, 1, 0), spotLightComp->Data().Diffuse()); + EXPECT_EQ(math::Color(0.3, 0.3, 0.3, 0.3), + spotLightComp->Data().Specular()); + EXPECT_NEAR(2.6, spotLightComp->Data().AttenuationRange(), 0.1); + EXPECT_NEAR(0.7, spotLightComp->Data().LinearAttenuationFactor(), 0.1); + EXPECT_NEAR(0.6, spotLightComp->Data().ConstantAttenuationFactor(), 0.1); + EXPECT_NEAR(1, spotLightComp->Data().QuadraticAttenuationFactor(), 0.1); + EXPECT_EQ(math::Vector3d(1, 2, 3), spotLightComp->Data().Direction()); + EXPECT_TRUE(spotLightComp->Data().CastShadows()); + EXPECT_EQ(sdf::LightType::SPOT, spotLightComp->Data().Type()); + EXPECT_NEAR(1.5, spotLightComp->Data().SpotInnerAngle().Radian(), 0.1); + EXPECT_NEAR(0.3, spotLightComp->Data().SpotOuterAngle().Radian(), 0.1); + EXPECT_NEAR(0.9, spotLightComp->Data().SpotFalloff(), 0.1); +} + +///////////////////////////////////////////////// +TEST_F(UserCommandsTest, Physics) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/shapes.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system just to get the ECM + EntityComponentManager *ecm{nullptr}; + test::Relay testSystem; + testSystem.OnPreUpdate([&](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }); + + server.AddSystem(testSystem.systemPtr); + + // Run server and check we have the ECM + EXPECT_EQ(nullptr, ecm); + server.Run(true, 1, false); + EXPECT_NE(nullptr, ecm); + + // Check that the physics properties are the ones specified in the sdf + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(kNullEntity, worldEntity); + auto physicsComp = ecm->Component(worldEntity); + ASSERT_NE(nullptr, physicsComp); + EXPECT_DOUBLE_EQ(0.001, physicsComp->Data().MaxStepSize()); + EXPECT_DOUBLE_EQ(1.0, physicsComp->Data().RealTimeFactor()); + + // Set physics properties + msgs::Physics req; + req.set_max_step_size(0.123); + req.set_real_time_factor(4.567); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/default/set_physics"}; + + transport::Node node; + 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 16b1a80498..df1354b924 100644 --- a/test/integration/velocity_control_system.cc +++ b/test/integration/velocity_control_system.cc @@ -97,43 +97,33 @@ class VelocityControlTest : public ::testing::TestWithParam // and max velocity (0.5 m/s). // See parameters // in "/test/worlds/velocity_control.sdf". - test::Relay velocityRamp; const double desiredLinVel = 10.5; const double desiredAngVel = 0.2; - velocityRamp.OnPreUpdate( - [&](const gazebo::UpdateInfo &/*_info*/, - const gazebo::EntityComponentManager &) - { - msgs::Set(msg.mutable_linear(), - math::Vector3d(desiredLinVel, 0, 0)); - msgs::Set(msg.mutable_angular(), - math::Vector3d(0.0, 0, desiredAngVel)); - pub.Publish(msg); - }); - - server.AddSystem(velocityRamp.systemPtr); + 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, poses.size()); - int sleep = 0; - int maxSleep = 30; - - ASSERT_NE(maxSleep, sleep); - // verify that the vehicle is moving in +x and rotating towards +y for (unsigned int i = 1001; i < poses.size(); ++i) { - EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()); - EXPECT_GT(poses[i].Pos().Y(), poses[i-1].Pos().Y()); + EXPECT_GT(poses[i].Pos().X(), poses[i-1].Pos().X()) << i; + EXPECT_GT(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); + poses[i-1].Rot().Euler().X(), 1e-5) << i; EXPECT_NEAR(poses[i].Rot().Euler().Y(), - poses[i-1].Rot().Euler().Y(), 1e-5); - EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()); + poses[i-1].Rot().Euler().Y(), 1e-5) << i; + EXPECT_GT(poses[i].Rot().Euler().Z(), poses[i-1].Rot().Euler().Z()) << i; } } }; diff --git a/test/performance/each.cc b/test/performance/each.cc index 272d04bff9..d67f343ac1 100644 --- a/test/performance/each.cc +++ b/test/performance/each.cc @@ -54,10 +54,10 @@ TEST(EntityComponentManagerPerfrormance, Each) // Initial allocation of resources can throw off calculations. warmstart(); - for (int matchingEntityCount = 1; matchingEntityCount < maxEntityCount; + for (int matchingEntityCount = 10; matchingEntityCount < maxEntityCount; matchingEntityCount += step) { - for (int nonmatchingEntityCount = 1; + for (int nonmatchingEntityCount = 10; nonmatchingEntityCount < maxEntityCount; nonmatchingEntityCount += step) { diff --git a/test/worlds/altimeter.sdf b/test/worlds/altimeter.sdf index fa0a098272..7b8c209632 100644 --- a/test/worlds/altimeter.sdf +++ b/test/worlds/altimeter.sdf @@ -21,6 +21,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/apply_joint_force.sdf b/test/worlds/apply_joint_force.sdf index d89570e396..bdc7e56f92 100644 --- a/test/worlds/apply_joint_force.sdf +++ b/test/worlds/apply_joint_force.sdf @@ -13,6 +13,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/breadcrumbs.sdf b/test/worlds/breadcrumbs.sdf index 855614c1f6..fae8d86898 100644 --- a/test/worlds/breadcrumbs.sdf +++ b/test/worlds/breadcrumbs.sdf @@ -40,6 +40,7 @@ 0 0 1 + 100 100 @@ -661,5 +662,3 @@ - - diff --git a/test/worlds/camera_video_record.sdf b/test/worlds/camera_video_record.sdf index d8fdf8f1e3..3a08ef9fa9 100644 --- a/test/worlds/camera_video_record.sdf +++ b/test/worlds/camera_video_record.sdf @@ -41,6 +41,7 @@ 0 0 1 + 100 100 @@ -307,4 +308,3 @@ - diff --git a/test/worlds/collada_world_exporter.sdf b/test/worlds/collada_world_exporter.sdf new file mode 100644 index 0000000000..b65709b628 --- /dev/null +++ b/test/worlds/collada_world_exporter.sdf @@ -0,0 +1,150 @@ + + + + + + + + + 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.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 2 + 0 + 0 + 2 + 0 + 2 + + 2.0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + diff --git a/test/worlds/depth_camera_sensor.sdf b/test/worlds/depth_camera_sensor.sdf index bd8c1a23b8..f6fd16a163 100644 --- a/test/worlds/depth_camera_sensor.sdf +++ b/test/worlds/depth_camera_sensor.sdf @@ -40,6 +40,7 @@ 20 20 0.1 diff --git a/test/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index 89aa2fd68c..821d5a1a0f 100644 --- a/test/worlds/detachable_joint.sdf +++ b/test/worlds/detachable_joint.sdf @@ -11,6 +11,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/diff_drive.sdf b/test/worlds/diff_drive.sdf index 3a95bb1e92..b2b00bb0c4 100644 --- a/test/worlds/diff_drive.sdf +++ b/test/worlds/diff_drive.sdf @@ -32,6 +32,7 @@ 0 0 1 + 100 100 @@ -240,4 +241,3 @@ - diff --git a/test/worlds/diff_drive_custom_frame_id.sdf b/test/worlds/diff_drive_custom_frame_id.sdf index 073bfc41b8..41d9f1b4b9 100644 --- a/test/worlds/diff_drive_custom_frame_id.sdf +++ b/test/worlds/diff_drive_custom_frame_id.sdf @@ -32,6 +32,7 @@ 0 0 1 + 100 100 @@ -242,4 +243,3 @@ - diff --git a/test/worlds/diff_drive_custom_tf_topic.sdf b/test/worlds/diff_drive_custom_tf_topic.sdf new file mode 100644 index 0000000000..131207f3ef --- /dev/null +++ b/test/worlds/diff_drive_custom_tf_topic.sdf @@ -0,0 +1,244 @@ + + + + + + 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.325 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 + + + + left_wheel_joint + right_wheel_joint + 1.25 + 0.3 + 1 + 0.5 + tf_foo + + + + + + + + + + + diff --git a/test/worlds/diff_drive_custom_topics.sdf b/test/worlds/diff_drive_custom_topics.sdf index 312b28e872..075c1c6744 100644 --- a/test/worlds/diff_drive_custom_topics.sdf +++ b/test/worlds/diff_drive_custom_topics.sdf @@ -32,6 +32,7 @@ 0 0 1 + 100 100 @@ -237,4 +238,3 @@ - diff --git a/test/worlds/diff_drive_limited_joint_pub.sdf b/test/worlds/diff_drive_limited_joint_pub.sdf index a32638a281..33baaea880 100644 --- a/test/worlds/diff_drive_limited_joint_pub.sdf +++ b/test/worlds/diff_drive_limited_joint_pub.sdf @@ -32,6 +32,7 @@ 0 0 1 + 100 100 @@ -244,4 +245,3 @@ - diff --git a/test/worlds/diff_drive_skid.sdf b/test/worlds/diff_drive_skid.sdf index 2b314d83d2..1adff231cf 100644 --- a/test/worlds/diff_drive_skid.sdf +++ b/test/worlds/diff_drive_skid.sdf @@ -32,6 +32,7 @@ 0 0 1 + 100 100 @@ -325,4 +326,3 @@ - diff --git a/test/worlds/falling.sdf b/test/worlds/falling.sdf index 18f780bba4..9e69c968e6 100644 --- a/test/worlds/falling.sdf +++ b/test/worlds/falling.sdf @@ -61,6 +61,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/friction.sdf b/test/worlds/friction.sdf index f78c10dcbb..f4ce49cf4d 100644 --- a/test/worlds/friction.sdf +++ b/test/worlds/friction.sdf @@ -164,6 +164,7 @@ 0 0 1 + 100 100 @@ -186,4 +187,3 @@ - diff --git a/test/worlds/gpu_lidar_sensor.sdf b/test/worlds/gpu_lidar_sensor.sdf index e72ef9c32a..a8748e419b 100644 --- a/test/worlds/gpu_lidar_sensor.sdf +++ b/test/worlds/gpu_lidar_sensor.sdf @@ -40,6 +40,7 @@ 20 20 0.1 diff --git a/test/worlds/imu.sdf b/test/worlds/imu.sdf index 1cf77984f9..4bd476afa6 100644 --- a/test/worlds/imu.sdf +++ b/test/worlds/imu.sdf @@ -22,6 +22,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf index f2509c1ad6..06b9dd566b 100644 --- a/test/worlds/joint_controller.sdf +++ b/test/worlds/joint_controller.sdf @@ -58,6 +58,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/joint_position_controller.sdf b/test/worlds/joint_position_controller.sdf index 9394c1ad6a..906521db60 100644 --- a/test/worlds/joint_position_controller.sdf +++ b/test/worlds/joint_position_controller.sdf @@ -58,6 +58,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/joint_trajectory_controller.sdf b/test/worlds/joint_trajectory_controller.sdf new file mode 100644 index 0000000000..4aaee68ca5 --- /dev/null +++ b/test/worlds/joint_trajectory_controller.sdf @@ -0,0 +1,420 @@ + + + + + + + + + + + + 0.4 0.4 0.4 + false + + + + + + + + + + 3D View + false + docked + + ogre + scene + 0 0 1 0 1.5708 0 + + + + + + World control + false + false + 50 + 100 + 1 + floating + + + + + + true + true + true + + + + + + World stats + false + false + 250 + 110 + 1 + floating + + + + + + true + true + true + true + + + + + + + + false + 5 5 5 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -1 -1 -1 + + + + + + + + 0 -0.5 0 0 1.5708 0 + + + world + RR_position_control_link0 + + + + + + + 0.025 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0.5 0 0 1 + 0.8 0 0 1 + 0.8 0 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_position_control_link0 + RR_position_control_link1 + + 1 0 0 + + 0.5 + + + + + 0 0 0.1 0 0 0 + RR_position_control_link1 + RR_position_control_link2 + + 1 0 0 + + 0.25 + + + + + + + RR_position_control_joint2 + 25 + 400 + 0.1 + -0.5 + 0.5 + -5 + 5 + + RR_position_control_joint1 + 50 + 600 + 0.8 + -1 + 1 + -10 + 10 + + + + + + + 0 0.5 0 0 1.5708 0 + + + world + RR_velocity_control_link0 + + + + + + + 0.025 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0.025 + + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.0125 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0 0 0.2 0 0 0 + + + 0.0125 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + 0 0 0.1 0 0 0 + + + + 0.01 + 0.2 + + + + + + + 0.01 + 0.2 + + + + 0 0.5 0 1 + 0 0.8 0 1 + 0 0.8 0 1 + + + + 0.1 + + 0.0003358 + 0.0003358 + 0.000005 + + + + + + 0 0 0 0 0 0 + RR_velocity_control_link0 + RR_velocity_control_link1 + + 1 0 0 + + + 0.02 + + + + 0 0 0.1 0 0 0 + RR_velocity_control_link1 + RR_velocity_control_link2 + + 1 0 0 + + + 0.01 + + + + + + test_custom_topic/velocity_control + + + 0.6 + 175 + -10 + 10 + + 0.1 + 200 + -5 + 5 + + + + + diff --git a/test/worlds/lift_drag.sdf b/test/worlds/lift_drag.sdf index b3c4021254..4926c4d9a8 100644 --- a/test/worlds/lift_drag.sdf +++ b/test/worlds/lift_drag.sdf @@ -15,6 +15,7 @@ 0 0 1 + 100 100 @@ -204,4 +205,3 @@ - diff --git a/test/worlds/lights.sdf b/test/worlds/lights.sdf index 5454c28fd6..6f5daa40af 100644 --- a/test/worlds/lights.sdf +++ b/test/worlds/lights.sdf @@ -13,7 +13,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - true 0 0 10 0 0 0 @@ -63,7 +62,7 @@ 0 0.0 0.0 0 0 0 - + 0.5 @@ -75,7 +74,6 @@ 0.3 0.3 0.3 1 - 0 0 1.0 0 0 0 0 0 1 1 @@ -88,9 +86,7 @@ false - - diff --git a/test/worlds/lights_render.sdf b/test/worlds/lights_render.sdf new file mode 100644 index 0000000000..3f4d677460 --- /dev/null +++ b/test/worlds/lights_render.sdf @@ -0,0 +1,116 @@ + + + + + 0.001 + 1.0 + + + + + + + ogre + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 100 + 0.9 + 0.01 + 0.001 + + 0.5 0.2 -0.9 + + + + 0 -1.5 3 0 0 0 + 1 0 0 1 + .1 .1 .1 1 + + 4 + 0.2 + 0.5 + 0.01 + + false + + + + 0 1.5 3 0 0 0 + 0 1 0 1 + .2 .2 .2 1 + + 5 + 0.3 + 0.4 + 0.001 + + 0 0 -1 + + 0.1 + 0.5 + 0.8 + + false + + + + 0 0.0 0.0 0 0 0 + + + + 1 0 1.3 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + false + camera + + + + + 0.5 + + + + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + 0.3 0.3 0.3 1 + + + + 0 0 1.0 0 0 0 + 0 0 1 1 + .1 .1 .1 1 + + 2 + 0.05 + 0.02 + 0.01 + + false + + + + + diff --git a/test/worlds/log_playback.sdf b/test/worlds/log_playback.sdf index 4ad6985c1a..b46b04d357 100644 --- a/test/worlds/log_playback.sdf +++ b/test/worlds/log_playback.sdf @@ -7,8 +7,6 @@ - - /tmp/log diff --git a/test/worlds/log_record_dbl_pendulum.sdf b/test/worlds/log_record_dbl_pendulum.sdf index b93844b5fc..6b79cc1a43 100644 --- a/test/worlds/log_record_dbl_pendulum.sdf +++ b/test/worlds/log_record_dbl_pendulum.sdf @@ -50,6 +50,7 @@ 0 0 1 + 100 100 @@ -294,4 +295,3 @@ - diff --git a/test/worlds/log_record_resources.sdf b/test/worlds/log_record_resources.sdf index 254c1e4f09..6bf9adb87c 100644 --- a/test/worlds/log_record_resources.sdf +++ b/test/worlds/log_record_resources.sdf @@ -63,6 +63,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/logical_camera_sensor.sdf b/test/worlds/logical_camera_sensor.sdf index fce03dfd7b..96623b7d8e 100644 --- a/test/worlds/logical_camera_sensor.sdf +++ b/test/worlds/logical_camera_sensor.sdf @@ -37,6 +37,7 @@ 20 20 0.1 diff --git a/test/worlds/magnetometer.sdf b/test/worlds/magnetometer.sdf index c34dfc87b1..9fb5bda4ca 100644 --- a/test/worlds/magnetometer.sdf +++ b/test/worlds/magnetometer.sdf @@ -22,6 +22,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/mesh.sdf b/test/worlds/mesh.sdf index 9dcc8ae815..dd2d47ce04 100644 --- a/test/worlds/mesh.sdf +++ b/test/worlds/mesh.sdf @@ -42,6 +42,7 @@ 20 20 0.1 diff --git a/test/worlds/nondefault_canonical.sdf b/test/worlds/nondefault_canonical.sdf index a03d8c88e5..56c303542d 100644 --- a/test/worlds/nondefault_canonical.sdf +++ b/test/worlds/nondefault_canonical.sdf @@ -61,6 +61,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/particle_emitter.sdf b/test/worlds/particle_emitter.sdf new file mode 100644 index 0000000000..1da40a4919 --- /dev/null +++ b/test/worlds/particle_emitter.sdf @@ -0,0 +1,148 @@ + + + + + + 0.001 + 1.0 + + + + + + + + 3D View + false + false + 0 + + + + + + + ogre + 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 + + + + + + + smoke_emitter + box + 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/performer_detector.sdf b/test/worlds/performer_detector.sdf index 2a093d0176..faa7cef57d 100644 --- a/test/worlds/performer_detector.sdf +++ b/test/worlds/performer_detector.sdf @@ -17,6 +17,7 @@ 0 0 1 + 100 100 @@ -387,4 +388,3 @@ - diff --git a/test/worlds/pose_publisher.sdf b/test/worlds/pose_publisher.sdf index a4d88b0517..fadf485754 100644 --- a/test/worlds/pose_publisher.sdf +++ b/test/worlds/pose_publisher.sdf @@ -35,6 +35,7 @@ 0 0 1 + 100 100 @@ -515,4 +516,3 @@ - diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf index 06b0ad57c9..e6c7aa5a7b 100644 --- a/test/worlds/quadcopter.sdf +++ b/test/worlds/quadcopter.sdf @@ -17,6 +17,7 @@ 0 0 1 + 100 100 @@ -327,4 +328,3 @@ - diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf index 9583a1fc8f..fc8adea119 100644 --- a/test/worlds/quadcopter_velocity_control.sdf +++ b/test/worlds/quadcopter_velocity_control.sdf @@ -17,6 +17,7 @@ 0 0 1 + 100 100 @@ -366,4 +367,3 @@ - diff --git a/test/worlds/revolute_joint.sdf b/test/worlds/revolute_joint.sdf index 3821ff0eee..fd9632f5cd 100644 --- a/test/worlds/revolute_joint.sdf +++ b/test/worlds/revolute_joint.sdf @@ -58,6 +58,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/rgbd_camera_sensor.sdf b/test/worlds/rgbd_camera_sensor.sdf index d7d1bcc66a..5759741e11 100644 --- a/test/worlds/rgbd_camera_sensor.sdf +++ b/test/worlds/rgbd_camera_sensor.sdf @@ -36,6 +36,7 @@ 20 20 0.1 diff --git a/test/worlds/shapes.sdf b/test/worlds/shapes.sdf index ac254fa80b..dad5d0a8db 100644 --- a/test/worlds/shapes.sdf +++ b/test/worlds/shapes.sdf @@ -64,6 +64,7 @@ + 1150 0.12 0.12 0.12 0 0 0 @@ -105,6 +106,7 @@ + 1654 0.22 0.22 0.22 0 0 0 @@ -146,6 +148,7 @@ + 50 0.32 0.32 0.32 0 0 0 0.5 false diff --git a/test/worlds/thermal.sdf b/test/worlds/thermal.sdf index 223b04ba51..5274b749b0 100644 --- a/test/worlds/thermal.sdf +++ b/test/worlds/thermal.sdf @@ -49,6 +49,7 @@ 20 20 0.1 @@ -74,93 +75,100 @@ - - 0 0 0.5 0 0 0 - + + 0 1.5 0.5 0 0 0 + - 1 + 3 0 0 - 1 + 3 0 - 1 + 3 - 1.0 + 3.0 - + - - 1 1 1 - + + 0.5 + - + - - 1 1 1 - + + 0.5 + - 1 0 0 1 - 1 0 0 1 - 1 0 0 1 + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 - 200.0 + 600.0 - - 0 1.5 0.5 0 0 0 - + + 0 -1.5 0.5 0 0 0 + - 3 + 2 0 0 - 3 + 2 0 - 3 + 2 - 3.0 + 2.0 - + - + 0.5 - + 1.0 + - + - + 0.5 - + 1.0 + - 0 0 1 1 - 0 0 1 1 - 0 0 1 1 + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 - 600.0 + 400.0 - - 0 -1.5 0.5 0 0 0 - + + + -2 -1.5 0.5 0 0 0 + 2 @@ -172,7 +180,7 @@ 2.0 - + 0.5 @@ -181,7 +189,7 @@ - + 0.5 @@ -196,10 +204,156 @@ + ../media/duck.png 400.0 + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + ../media/duck.png + 500.0 + + + + + + + + 0 1.5 0.5 0 0 0 + + + + 3 + 0 + 0 + 3 + 0 + 3 + + 3.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + ../media/duck.png + 400.0 + + + + + + + + 0 0 0 0 0 1.570796 + rescue_randy + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Rescue Randy + + + + 4.5 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_8bit/image + + 253.15 + 673.15 + 3.0 + + + + true + diff --git a/test/worlds/thermal_invalid.sdf b/test/worlds/thermal_invalid.sdf new file mode 100644 index 0000000000..89c621b9c9 --- /dev/null +++ b/test/worlds/thermal_invalid.sdf @@ -0,0 +1,152 @@ + + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + -0.5 0.1 -0.9 + + + + 310 + + + + true` + 0 0.5 0.5 0 0 0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + 600.0 + + + + + + + true` + 0 -0.5 0.5 0 0 0 + + + + + 0.5 + 1.0 + + + + + + + + 0.5 + 1.0 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + 800.0 + + + + + + + 1.0 0 0.5 0.0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + L8 + + + 0.1 + 100 + + + 1 + 30 + true + thermal_camera_invalid/image + + 999 + -590 + 0.0 + + + + true + + + diff --git a/test/worlds/touch_plugin.sdf b/test/worlds/touch_plugin.sdf index 90a802f623..4ff260b7f3 100644 --- a/test/worlds/touch_plugin.sdf +++ b/test/worlds/touch_plugin.sdf @@ -17,6 +17,7 @@ 0 0 1 + 100 100 @@ -212,4 +213,3 @@ - diff --git a/test/worlds/triball_drift.sdf b/test/worlds/triball_drift.sdf index bba019b21b..d574e2a2b6 100644 --- a/test/worlds/triball_drift.sdf +++ b/test/worlds/triball_drift.sdf @@ -92,6 +92,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/trisphere_cycle_wheel_slip.sdf b/test/worlds/trisphere_cycle_wheel_slip.sdf index cf0af7d524..b086b05a0f 100644 --- a/test/worlds/trisphere_cycle_wheel_slip.sdf +++ b/test/worlds/trisphere_cycle_wheel_slip.sdf @@ -92,6 +92,7 @@ 0 0 1 + 100 100 diff --git a/test/worlds/velocity_control.sdf b/test/worlds/velocity_control.sdf index fcfabd8c12..2e104e5f08 100644 --- a/test/worlds/velocity_control.sdf +++ b/test/worlds/velocity_control.sdf @@ -119,6 +119,7 @@ 0 0 1 + 100 100 diff --git a/tutorials.md.in b/tutorials.md.in index aa37f6b31d..b02a44fbe7 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -13,8 +13,10 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage levels "Levels": Load entities on demand in large environments. * \subpage distributedsimulation "Distributed Simulation": Spread simulation across several processes. * \subpage resources "Finding resources": The different ways in which Ignition looks for files. +* \subpage entity_creation "Entity creation": Insert models or lights using services. * \subpage log "Logging": Record and play back time series of world state. * \subpage physics "Physics engines": Loading different physics engines. +* \subpage light_config "Light config": Configure lights in the scene. * \subpage battery "Battery": Keep track of battery charge on robot models. * \subpage gui_config "GUI configuration": Customizing your layout. * \subpage server_config "Server configuration": Customizing what system plugins are loaded. @@ -25,10 +27,13 @@ Ignition @IGN_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage triggeredpublisher "Triggered Publisher": Using the TriggeredPublisher system to orchestrate actions in simulation. * \subpage logicalaudiosensor "Logical Audio Sensor": Using the LogicalAudioSensor system to mimic logical audio emission and detection in simulation. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. +* \subpage collada_world_exporter "Collada World Exporter": Export an entire +world to a single Collada mesh. **Migration from Gazebo classic** * \subpage migrationplugins "Plugins": Walk through the differences between writing plugins for Gazebo classic and Ignition Gazebo +* \subpage migrationsdf "SDF": Migrating SDF files from Gazebo classic to Ignition Gazebo * \subpage migrationworldapi "World API": Guide on what World C++ functions to call in Ignition Gazebo when migrating from Gazebo classic * \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Ignition Gazebo when migrating from Gazebo classic * \subpage migrationlinkapi "Link API": Guide on what Link C++ functions to call in Ignition Gazebo when migrating from Gazebo classic diff --git a/tutorials/collada_world_exporter.md b/tutorials/collada_world_exporter.md new file mode 100644 index 0000000000..e1bdd4e60a --- /dev/null +++ b/tutorials/collada_world_exporter.md @@ -0,0 +1,25 @@ +\page collada_world_exporter Collada World Exporter + +The Collada world exporter is a world plugin that automatically generates +a Collada mesh consisting of the models within a world SDF file. This plugin +can be useful if you'd like to share an SDF world without requiring an SDF +loader. + +## Using the Collada World Exporter + +1. Add the following lines as a child to the `` tag in an SDF file. +``` + + +``` + +2. Run the world using +``` +ign gazebo -v 4 -s -r --iterations 1 WORLD_FILE_NAME +``` + +3. A subdirectory, named after the world, has been created in the current working directory. Within this subdirectory is the mesh and materials for the world. + +Refer to the [collada_world_exporter.sdf](https://github.com/ignitionrobotics/ign-gazebo/blob/ign-gazebo4/examples/worlds/collada_world_exporter.sdf) example. diff --git a/tutorials/entity_creation.md b/tutorials/entity_creation.md new file mode 100644 index 0000000000..0bbd34df56 --- /dev/null +++ b/tutorials/entity_creation.md @@ -0,0 +1,111 @@ +\page entity_creation Entity creation + +This tutorial gives an introduction to Ignition Gazebo's service `/world//create`. +This service allows creating entities in the scene such us spheres, lights, etc. + +Ignition Gazebo creates many services depending on the plugins that are specified in the SDF. +In this case we need to load the `UserCommands` plugin, which will offer the `create` service. +You can include the `UserCommands` system plugin including these lines in your SDF: + +```xml + + +``` + +You can check if this service is available typing: + +In one terminal + +```bash +ign gazebo -r -v 4 .sdf +``` + +In another terminal, see if the create service is listed: + +```bash +ign service --list +/gazebo/resource_paths/add +/gazebo/resource_paths/get +/gazebo/worlds +/gui/follow +/gui/move_to +/gui/move_to/pose +/gui/record_video +/gui/transform_mode +/gui/view_angle +/marker +/marker/list +/marker_array +/server_control +/world/world_name/control +/world/world_name/create +/world/world_name/create_multiple +/world/world_name/generate_world_sdf +/world/world_name/gui/info +/world/world_name/level/set_performer +/world/world_name/light_config +/world/world_name/playback/control +/world/world_name/remove +/world/world_name/scene/graph +/world/world_name/scene/info +/world/world_name/set_pose +/world/world_name/state +/world/world_name/state_async +``` + +# Factory message + +To create new entities in the world we need to use the +[ignition::msgs::EntityFactory](https://ignitionrobotics.org/api/msgs/6.0/classignition_1_1msgs_1_1EntityFactory__V.html) +message to send a request to the create service. +This message allows us to create entities from strings, files, +[Models](https://ignitionrobotics.org/api/msgs/6.0/classignition_1_1msgs_1_1Model.html), +[Lights](https://ignitionrobotics.org/api/msgs/6.0/classignition_1_1msgs_1_1Light.html) or even clone models. +This tutorial introduces how to create entities from SDF strings and light messages. + +## Insert an entity based on a string + +We will open an empty Ignition Gazebo world, let's start creating a sphere in the world. +In the next snippet you can see how to create models based on strings. + +\snippet examples/standalone/model_creation/model_creation.cc create sphere + +The variable `sphereStr` contains the SDF of the model that we want to create in the world. +In this case we are creating a sphere of 1 meter of radius in the position: `x: 0 y: 0 z: 0.5 roll: 0 pitch: 0 yaw: 0`. + +**NOTE**: You can insert here all kinds of models that can be described using an SDF string. + +Then we need to call the service `/world//create`: + +\snippet examples/standalone/model_creation/model_creation.cc call service create sphere + +**NOTE**: By default, if the entity name does not exist then the entity will be created +in the world. On the other hand, if that entity name already exists, then nothing will +happen. You may see some traces in the console showing this information. +There is an option to create a new entity every time that the message is sent by setting +`allow_renaming` to true (you can use the method `set_allow_renaming()`). + +## Insert a light + +To insert a light in the world we have two options: + + - Fill the string inside the `ignition::msgs::EntityFactory` message like in the section above. + - Fill the field `light` inside the `ignition::msgs::EntityFactory` message. + +In the following snippet you can see how the light's field is filled. + +\snippet examples/standalone/model_creation/model_creation.cc create light + +Or we can create an SDF string: + +\snippet examples/standalone/model_creation/model_creation.cc create light str + +Please check the API to know which fields are available in the +[Light message](https://ignitionrobotics.org/api/msgs/6.2/classignition_1_1msgs_1_1Light.html). +There are three types of lights: Point, Directional and Spot. + +Finally we should call the same service `/world//create`: + +\snippet examples/standalone/model_creation/model_creation.cc call service create diff --git a/tutorials/light_config.md b/tutorials/light_config.md new file mode 100644 index 0000000000..8a87170faf --- /dev/null +++ b/tutorials/light_config.md @@ -0,0 +1,45 @@ +\page light_config Light config + +This tutorial gives an introduction to Ignition Gazebo's service `/world//light_config`. +This service will allow to modify lights in the scene. + +# Modifying lights + +To modify lights inside the scene we need to use the service `/world//light_config` and +fill the message [`ignition::msgs::Light`](https://ignitionrobotics.org/api/msgs/6.0/classignition_1_1msgs_1_1Light.html). +In particular this example modifies the point light that we introduced with the function `createLight()`. + +\snippet examples/standalone/light_control/light_control.cc create light + +**NOTE:**: You can check the [entity creation](entity_creation.html) tutorial to learn how to include models and lights in the scene. + +As you can see in the snippet we modify the specular and diffuse colors of the light in the scene. + +\snippet examples/standalone/light_control/light_control.cc modify light + +In this case we are creating random numbers to fill the diffuse and specular. + +\snippet examples/standalone/light_control/light_control.cc random numbers + +# Run the example + +To run this example you should `cd` into `examples/standalone/light_control` and build the code: + +```bash +mkdir build +cd build +cmake .. +make +``` + +Then you should open two terminals and run: + + - Terminal one: + ```bash + ign gazebo -r -v 4 empty.sdf + ``` + + - Terminal two: + ```bash + ./light_control + ``` diff --git a/tutorials/log.md b/tutorials/log.md index 4a69e41eef..09423fa8a7 100644 --- a/tutorials/log.md +++ b/tutorials/log.md @@ -96,6 +96,11 @@ directory specified to record: `ign gazebo -r -v 4 --playback ` +### From plugin in SDF + +Playing back via the SDF tag `` has been removed. +Please use the command line argument. + ## Known issues * When using command-line playback there is currently a small caveat. diff --git a/tutorials/migration_plugins.md b/tutorials/migration_plugins.md index a84271249b..1ea353b5b7 100644 --- a/tutorials/migration_plugins.md +++ b/tutorials/migration_plugins.md @@ -75,7 +75,7 @@ class MyPlugin : public ModelPlugin this->link = _model->GetLink(linkName); // Register callback to be called at every iteration - this->connection = event::Events::ConnectWorldUpdateBegin( + this->updateConnection = event::Events::ConnectWorldUpdateBegin( std::bind(&MyPlugin::OnUpdate, this)); } diff --git a/tutorials/migration_sdf.md b/tutorials/migration_sdf.md new file mode 100644 index 0000000000..52e3716bb1 --- /dev/null +++ b/tutorials/migration_sdf.md @@ -0,0 +1,341 @@ +\page migrationsdf + +# Migration from Gazebo classic: SDF + +Both Gazebo classic and Ignition Gazebo support [SDF](http://sdformat.org/) +files to describe the simulation to be loaded. An SDF file defines the world +environment, the robot's characteristics and what plugins to load. + +Despite using the same description format, users will note that the same SDF +file may behave differently for each simulator. This tutorial will +explain how to write SDF files in a way that they're as reusable by both +simulators as possible. It will also explain when you'll need to use separate +files for each simulator. + +The minimum required versions to use this guide are: + +* Gazebo 11.2.0 +* Igniton Citadel + +## URIs + +SDF files use URIs to refer to resources from other files, such as meshes and +nested models. These are some of the SDF tags that take URIs: + +* `` +* `` +* `<...><*_map>` (only on Ignition) +* `` +* `` + +Here are the recommended ways to use URIs from most recommended to least: + +### Ignition Fuel URL + +It's possible to use URLs of resources on +[Ignition Fuel](https://app.ignitionrobotics.org) within any of the tags +above and both simulators will be able to load it. + +For example, this world can be loaded into both simulators: + +``` + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + + + + 3 -1.5 0 0 0 0 + true + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + + + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + + + + + + + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + 1.0 + + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + 1.0 + + + + + + +``` + +\note The actor's vertical pose will be different on both simulators. + That's because a hardcoded offset was removed on Ignition and + maintained on Gazebo classic for backwards compatibility. + +### Path relative to the SDF file + +It's possible to use relative paths within SDF files to refer to other files +in the same directory. This is recommended when creating files that work +together and need to be relocatable. + +For example, consider the following directory structure: + +``` +/home/username/ +├── world.sdf +└── models + └── model1 + ├── model.sdf + └── meshes + └── mesh.dae +``` + +The world `world.sdf` can include `model1` as follows: + +`models/model1` + +And `model.sdf` can refer to `mesh.dae` as: + +`meshes/mesh.dae` + +### Path relative to an environment variable + +This method is useful if you don't know where the files will be located +with respect to each other, and you have some control over the simulation's +runtime environment. On either simulator, you can refer to resources relative +to paths set on an environment variable. + +Each simulator uses a different environment variable: + +* Gazebo classic: + * `GAZEBO_MODEL_PATH` for models + * `GAZEBO_RESOURCE_PATH` for worlds and some rendering resources +* Ignition Gazebo: + * `IGN_GAZEBO_RESOURCE_PATH` for worlds, models and other resources + +For example, if you have the file structure above, you can set the environment +variable to `/home/username/models`: + +``` +export GAZEBO_MODEL_PATH=/home/username/models +export GAZEBO_RESOURCE_PATH=/home/username/models +export IGN_GAZEBO_RESOURCE_PATH=/home/username/models +``` + +And inside `world.sdf` include the model with: + +`model://model1` + +Also, `model.sdf` can refer to `mesh.dae` as: + +`model://model1/meshes/mesh.dae` + +On both situations, the `model://` prefix will be substituted by +`/home/username/models`. + +You can also set several lookup paths separating them with `:`, for example: + +`export IGN_GAZEBO_RESOURCE_PATH=/home/username/models:/home/username/another_project/models` + +### Absolute paths + +Finally, both simulators will accept absolute paths, for example: + +`/home/username/models/model1` + +This method is not recommended, because the file most likely won't work if +copied to a different computer. It also becomes inconvenient to move files to +different directories. + +## Plugins + +Plugins are binary files compiled to use with a specific simulator. Plugins +for Gazebo classic and Ignition Gazebo aren't usually compatible, so plugins +will need to be specified for each simulator separately. + +It's important to note that for both simulators, plugins compiled against +a major version of the simulator can't be loaded by other major versions. +The shared libraries will usually have the same name, so it's important to make +sure you're loading the correct plugins. + +### Official plugins + +Both simulators are installed with several built-in plugins. +[Gazebo classic's plugins](https://github.com/osrf/gazebo/tree/gazebo11/plugins) +and +[Ignition Gazebo's plugins](https://github.com/ignitionrobotics/ign-gazebo/tree/ign-gazebo3/src/systems) +have different file names. For example, to use Gazebo classic's differential drive +plugin, the user can refer to it as follows: + +``` + + + ... + + +``` + +On Ignition, that would be: + +``` + + + ... + + +``` + +Note that besides the different file name, Ignition also requires the C++ class +to be defined in the `name` attribute. + +Also keep in mind that plugins that offer similar functionality may accept +different parameters for each simulator. Be sure to check the documentation of +each plugin before using it. + +### Custom plugins + +To load custom plugins, users need to set environment variables to the directory +where that plugin is located. The variables are different for each simulator: + +* Gazebo classic: + * `GAZEBO_PLUGIN_PATH` for all plugin types. +* Ignition Gazebo: + * `IGN_GAZEBO_SYSTEM_PLUGIN_PATH` for Ignition Gazebo systems (world, model, + sensor and visual plugins). + * `IGN_GUI_PLUGIN_PATH` for GUI plugins. + +### Keeping plugins separate + +Trying to load a plugin from one simulator into the other will: + +* Print an error message if the simulator can't find the plugin +* Potentially crash if the simulator can find the plugin and tries to load it + +That's why it's recommended not to specify plugins for both simulators +side-by-side on the same file. Instead, keep separate files and inject the plugins as +needed. + +There isn't a built-in mechanism on SDFormat to inject plugins into files yet, +but users can make use of templating tools like [ERB](erb_template.html) +and [xacro](http://wiki.ros.org/xacro) to generate SDF files with the correct plugins. + +### Default plugins + +Ignition Gazebo is more modular than Gazebo classic, so most features are optional. +For example, by default, Ignition will load all the system plugins defined on +the `~/.ignition/gazebo/server.config` file and all GUI plugins defined on the +`~/.ignition/gazebo/gui.config` file. But the user can always remove plugins from +those files, or choose different ones by adding `` tags to the SDF file. +(For more details, see the [Server configuration tutorial](server_config.html) +and the [GUI configuration tutorial](gui_config.html)). + +This is important to keep in mind when migrating your SDF files, because files +that worked on Gazebo classic may need more plugins on Ignition. + +## Materials + +Ignition does not support Ogre material files like Classic does, because Ignition +Gazebo can be used with multiple rendering engines. Therefore, materials defined +within a ` + +``` + +To make your models compatible with both simulators, you can use these alternatives: + +### Plain colors + +If the material defines plain colors, use the ``, ``, +`` and `` tags as needed. + +For example, this material from +[gazebo.material](https://github.com/osrf/gazebo/blob/gazebo11/media/materials/scripts/gazebo.material): + +``` + + + +``` + +Can be changed to: + +``` + + 1 0 0 1 + 1 0 0 1 + 0.1 0.1 0.1 1 + + +``` + +### Textures + +If an Ogre material script is being used to define a texture, there are a +couple alternatives. + +If using mesh files, the texture can be embedded into it. The advantage is that +this works for both simulators. Some examples: + +* [OBJ + MTL](https://app.ignitionrobotics.org/OpenRobotics/fuel/models/DeskChair) +* [COLLADA](https://app.ignitionrobotics.org/OpenRobotics/fuel/models/Lamp%20Post) + +For primitive shapes or even meshes, you can pass the texture as the albedo map. If you +want the model to be compatible with both Classic and Ignition, you can specify both +the script and the albedo map. + +``` + + + + texture.png + + + +``` + diff --git a/tutorials/video_recorder.md b/tutorials/video_recorder.md index a16442e894..2a0f6f05fd 100644 --- a/tutorials/video_recorder.md +++ b/tutorials/video_recorder.md @@ -105,3 +105,10 @@ going to the world SDF file, locate the * **bitrate**: Video encoding bitrate in bps. This affects the quality of the generated video. The default bitrate is 2Mbps. + +## Hardware-accelerated encoding + +Since Ignition Common 3.10.2, there is support for utilizing the power of GPUs +to speed up the video encoding process. See the +[Hardware-accelerated Video Encoding tutorial](https://ignitionrobotics.org/api/common/3.10/hw-encoding.html) +for more details.