diff --git a/.github/ci/packages.apt b/.github/ci/packages.apt index d834a78a77e..4a15aa2ddf6 100644 --- a/.github/ci/packages.apt +++ b/.github/ci/packages.apt @@ -3,22 +3,22 @@ libfreeimage-dev libglew-dev libignition-cmake2-dev libignition-common4-dev -libignition-gui5-dev -libignition-fuel-tools6-dev +libignition-gui6-dev +libignition-fuel-tools7-dev libignition-math6-eigen3-dev -libignition-msgs7-dev -libignition-physics4-dev +libignition-msgs8-dev +libignition-physics5-dev libignition-plugin-dev -libignition-rendering5-dev -libignition-sensors5-dev +libignition-rendering6-dev +libignition-sensors6-dev libignition-tools-dev -libignition-transport10-dev +libignition-transport11-dev libignition-utils1-cli-dev libogre-1.9-dev libogre-2.1-dev libprotobuf-dev libprotoc-dev -libsdformat11-dev +libsdformat12-dev libtinyxml2-dev libxi-dev libxmu-dev diff --git a/CMakeLists.txt b/CMakeLists.txt index a0b0d3df539..94162ecd4ab 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo5 VERSION 5.1.0) +project(ignition-gazebo6 VERSION 6.2.0) #============================================================================ # Find ignition-cmake @@ -34,18 +34,24 @@ else() set (EXTRA_TEST_LIB_DEPS) endif() +include(test/find_dri.cmake) +FindDRI() + #============================================================================ # Search for project-specific dependencies #============================================================================ +# Setting this policy enables using the protobuf_MODULE_COMPATIBLE +# set command in CMake versions older than 13.13 +set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # This option is needed to use the PROTOBUF_GENERATE_CPP # in case protobuf is found with the CMake config files # It needs to be set before any find_package(...) call # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat11 REQUIRED VERSION 11.2.1) -set(SDF_VER ${sdformat11_VERSION_MAJOR}) +ign_find_package(sdformat12 REQUIRED) +set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- # Find ignition-plugin @@ -54,18 +60,18 @@ set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) #-------------------------------------- # Find ignition-transport -ign_find_package(ignition-transport10 REQUIRED COMPONENTS log) -set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) +ign_find_package(ignition-transport11 REQUIRED COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) #-------------------------------------- # Find ignition-msgs -ign_find_package(ignition-msgs7 REQUIRED VERSION 7.1) -set(IGN_MSGS_VER ${ignition-msgs7_VERSION_MAJOR}) +ign_find_package(ignition-msgs8 REQUIRED) +set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) #-------------------------------------- # Find ignition-common # Always use the profiler component to get the headers, regardless of status. -ign_find_package(ignition-common4 VERSION 4.2 +ign_find_package(ignition-common4 VERSION 4.4 COMPONENTS profiler events @@ -76,13 +82,13 @@ set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) #-------------------------------------- # Find ignition-fuel_tools -ign_find_package(ignition-fuel_tools6 REQUIRED) -set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools6_VERSION_MAJOR}) +ign_find_package(ignition-fuel_tools7 REQUIRED) +set(IGN_FUEL_TOOLS_VER ${ignition-fuel_tools7_VERSION_MAJOR}) #-------------------------------------- # Find ignition-gui -ign_find_package(ignition-gui5 REQUIRED VERSION 5.2) -set(IGN_GUI_VER ${ignition-gui5_VERSION_MAJOR}) +ign_find_package(ignition-gui6 REQUIRED VERSION 6.1) +set(IGN_GUI_VER ${ignition-gui6_VERSION_MAJOR}) ign_find_package (Qt5 COMPONENTS Core @@ -93,40 +99,50 @@ ign_find_package (Qt5 #-------------------------------------- # Find ignition-physics -ign_find_package(ignition-physics4 +ign_find_package(ignition-physics5 VERSION 5.1 COMPONENTS heightmap mesh sdf REQUIRED ) -set(IGN_PHYSICS_VER ${ignition-physics4_VERSION_MAJOR}) +set(IGN_PHYSICS_VER ${ignition-physics5_VERSION_MAJOR}) #-------------------------------------- # Find ignition-sensors -ign_find_package(ignition-sensors5 REQUIRED +ign_find_package(ignition-sensors6 REQUIRED + # component order is important COMPONENTS - rendering + # non-rendering air_pressure altimeter - camera - gpu_lidar imu + force_torque logical_camera magnetometer + + # rendering + rendering + lidar + gpu_lidar + + # cameras + camera + segmentation_camera depth_camera + rgbd_camera thermal_camera ) -set(IGN_SENSORS_VER ${ignition-sensors5_VERSION_MAJOR}) +set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-rendering -ign_find_package(ignition-rendering5 REQUIRED VERSION 5.1) -set(IGN_RENDERING_VER ${ignition-rendering5_VERSION_MAJOR}) +ign_find_package(ignition-rendering6 REQUIRED) +set(IGN_RENDERING_VER ${ignition-rendering6_VERSION_MAJOR}) #-------------------------------------- # Find ignition-math -ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.8) +ign_find_package(ignition-math6 REQUIRED COMPONENTS eigen3 VERSION 6.9) set(IGN_MATH_VER ${ignition-math6_VERSION_MAJOR}) #-------------------------------------- @@ -148,7 +164,7 @@ ign_find_package(IgnProtobuf REQUIRED COMPONENTS all PRETTY Protobuf) -set(Protobuf_IMPORT_DIRS ${ignition-msgs7_INCLUDE_DIRS}) +set(Protobuf_IMPORT_DIRS ${ignition-msgs8_INCLUDE_DIRS}) # Plugin install dirs set(IGNITION_GAZEBO_PLUGIN_INSTALL_DIR diff --git a/Changelog.md b/Changelog.md index 185e3abcf13..b4a2011901b 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,6 +1,619 @@ +## Ignition Gazebo 6.x + +### Ignition Gazebo 6.X.X (202X-XX-XX) + +### Ignition Gazebo 6.2.0 (2021-11-16) + +1. Configurable joint state publisher's topic + * [Pull request #1076](https://github.com/ignitionrobotics/ign-gazebo/pull/1076) + +1. Thruster plugin: add tests and velocity control + * [Pull request #1190](https://github.com/ignitionrobotics/ign-gazebo/pull/1190) + +1. Prevent creation of spurious `` elements when saving worlds + * [Pull request #1192](https://github.com/ignitionrobotics/ign-gazebo/pull/1192) + +1. Add `sdfString` to `ServerConfig`'s copy constructor. + * [Pull request #1185](https://github.com/ignitionrobotics/ign-gazebo/pull/1185) + +1. Added support for tracked vehicles + * [Pull request #869](https://github.com/ignitionrobotics/ign-gazebo/pull/869) + +1. Add components to dynamically set joint limits + * [Pull request #847](https://github.com/ignitionrobotics/ign-gazebo/pull/847) + +1. Remove bounding box when entities are removed + * [Pull request #1053](https://github.com/ignitionrobotics/ign-gazebo/pull/1053) + * [Pull request #1213](https://github.com/ignitionrobotics/ign-gazebo/pull/1213) + +1. Fix updating component from state + * [Pull request #1181](https://github.com/ignitionrobotics/ign-gazebo/pull/1181) + +1. Extend odom publisher to allow 3D + * [Pull request #1180](https://github.com/ignitionrobotics/ign-gazebo/pull/1180) + +1. Support copy/paste + * [Pull request #1013](https://github.com/ignitionrobotics/ign-gazebo/pull/1013) + +1. Tweaks install instructions + * [Pull request #1078](https://github.com/ignitionrobotics/ign-gazebo/pull/1078) + +1. Publish 10 world stats msgs/sec instead of 5 + * [Pull request #1163](https://github.com/ignitionrobotics/ign-gazebo/pull/1163) + +1. Add functionality to add entities via the entity tree + * [Pull request #1101](https://github.com/ignitionrobotics/ign-gazebo/pull/1101) + +1. Get updated GUI ECM info when a user presses 'play' + * [Pull request #1109](https://github.com/ignitionrobotics/ign-gazebo/pull/1109) + +1. Create expanding type header to reduce code duplication + * [Pull request #1169](https://github.com/ignitionrobotics/ign-gazebo/pull/1169) + +1. `minimal_scene.sdf` example: add `camera_clip` params + * [Pull request #1166](https://github.com/ignitionrobotics/ign-gazebo/pull/1166) + +1. Sensor systems work if loaded after sensors + * [Pull request #1104](https://github.com/ignitionrobotics/ign-gazebo/pull/1104) + +1. Support printing sensors using `ign model` + * [Pull request #1157](https://github.com/ignitionrobotics/ign-gazebo/pull/1157) + +1. Set camera clipping plane distances from the GUI + * [Pull request #1162](https://github.com/ignitionrobotics/ign-gazebo/pull/1162) + +1. Fix generation of systems library symlinks in build directory + * [Pull request #1160](https://github.com/ignitionrobotics/ign-gazebo/pull/1160) + +1. Add a default value for `isHeadlessRendering`. + * [Pull request #1151](https://github.com/ignitionrobotics/ign-gazebo/pull/1151) + +1. Component inspector + + 1. Edit material colors + * [Pull request #1123](https://github.com/ignitionrobotics/ign-gazebo/pull/1123) + * [Pull request #1186](https://github.com/ignitionrobotics/ign-gazebo/pull/1186) + + 1. Fix integers and floats + * [Pull request #1143](https://github.com/ignitionrobotics/ign-gazebo/pull/1143) + + 1. Prevent a segfault when updating + * [Pull request #1167](https://github.com/ignitionrobotics/ign-gazebo/pull/1167) + + 1. Use `uint64_t` for Entity IDs + * [Pull request #1144](https://github.com/ignitionrobotics/ign-gazebo/pull/1144) + +1. Support setting the background color for sensors + * [Pull request #1147](https://github.com/ignitionrobotics/ign-gazebo/pull/1147) + +1. Select top level entity not visual + * [Pull request #1150](https://github.com/ignitionrobotics/ign-gazebo/pull/1150) + +1. Update create entity offset on GUI side + * [Pull request #1145](https://github.com/ignitionrobotics/ign-gazebo/pull/1145) + +1. Update Select Entities GUI plugin to use Entity type + * [Pull request #1146](https://github.com/ignitionrobotics/ign-gazebo/pull/1146) + +1. Notify other GUI plugins of added/removed entities via GUI events + * [Pull request #1138](https://github.com/ignitionrobotics/ign-gazebo/pull/1138) + * [Pull request #1213](https://github.com/ignitionrobotics/ign-gazebo/pull/1213) + +### Ignition Gazebo 6.1.0 (2021-10-25) + +1. Updates to camera video record from subt + * [Pull request #1117](https://github.com/ignitionrobotics/ign-gazebo/pull/1117) + +1. Use the actor tension parameter + * [Pull request #1091](https://github.com/ignitionrobotics/ign-gazebo/pull/1091) + +1. Better protect this->dataPtr->initialized with renderMutex. + * [Pull request #1119](https://github.com/ignitionrobotics/ign-gazebo/pull/1119) + +1. Use QTimer to update plugins in the Qt thread + * [Pull request #1095](https://github.com/ignitionrobotics/ign-gazebo/pull/1095) + +1. Adjust pose decimals based on element width + * [Pull request #1089](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. JointPositionController: Improve misleading error message + * [Pull request #1098](https://github.com/ignitionrobotics/ign-gazebo/pull/1098) + +1. Fixed IMU system plugin + * [Pull request #1043](https://github.com/ignitionrobotics/ign-gazebo/pull/1043) + +1. Prevent crash and print error + * [Pull request #1099](https://github.com/ignitionrobotics/ign-gazebo/pull/1099) + +1. Create GUI config folder before copying config + * [Pull request #1092](https://github.com/ignitionrobotics/ign-gazebo/pull/1092) + +1. Add support for configuring point size in Visualize Lidar GUI plugin + * [Pull request #1021](https://github.com/ignitionrobotics/ign-gazebo/pull/1021) + +1. Set a cloned joint's parent/child link names to the cloned parent/child link names + * [Pull request #1075](https://github.com/ignitionrobotics/ign-gazebo/pull/1075) + +1. Performance: use std::unordered_map where possible in SceneManager + * [Pull request #1083](https://github.com/ignitionrobotics/ign-gazebo/pull/1083) + +1. Fix transform controls + * [Pull request #1081](https://github.com/ignitionrobotics/ign-gazebo/pull/1081) + +1. Fix View Angle's home button + * [Pull request #1082](https://github.com/ignitionrobotics/ign-gazebo/pull/1082) + +1. Fix light control standalone example + * [Pull request #1077](https://github.com/ignitionrobotics/ign-gazebo/pull/1077) + +1. Parse new param for enabling / disabling IMU orientation output + * [Pull request #899](https://github.com/ignitionrobotics/ign-gazebo/pull/899) + +### Ignition Gazebo 6.0.0 (2021-10-01) + +1. Deprecated GzScene3D in favor of MinimalScene + * [Pull request #1065](https://github.com/ignitionrobotics/ign-gazebo/pull/1065) + * [Pull request #1051](https://github.com/ignitionrobotics/ign-gazebo/pull/1051) + * [Pull request #1014](https://github.com/ignitionrobotics/ign-gazebo/pull/1014) + * [Pull request #1034](https://github.com/ignitionrobotics/ign-gazebo/pull/1034) + * [Pull request #900](https://github.com/ignitionrobotics/ign-gazebo/pull/900) + * [Pull request #988](https://github.com/ignitionrobotics/ign-gazebo/pull/988) + * [Pull request #1016](https://github.com/ignitionrobotics/ign-gazebo/pull/1016) + * [Pull request #983](https://github.com/ignitionrobotics/ign-gazebo/pull/983) + * [Pull request #854](https://github.com/ignitionrobotics/ign-gazebo/pull/854) + * [Pull request #813](https://github.com/ignitionrobotics/ign-gazebo/pull/813) + * [Pull request #905](https://github.com/ignitionrobotics/ign-gazebo/pull/905) + +1. Fix GuiRunner initial state and entity spawn timing issue + * [Pull request #1073](https://github.com/ignitionrobotics/ign-gazebo/pull/1073) + +1. Buoyancy plugin upgrade + * [Pull request #818](https://github.com/ignitionrobotics/ign-gazebo/pull/818) + * [Pull request #1067](https://github.com/ignitionrobotics/ign-gazebo/pull/1067) + * [Pull request #1064](https://github.com/ignitionrobotics/ign-gazebo/pull/1064) + +1. Fix non desired window opening alongside ignition GUI + * [Pull request #1063](https://github.com/ignitionrobotics/ign-gazebo/pull/1063) + +1. Documentation + * [Pull request #1074](https://github.com/ignitionrobotics/ign-gazebo/pull/1074) + * [Pull request #996](https://github.com/ignitionrobotics/ign-gazebo/pull/996) + +1. Update to latest SDFormat changes + * [Pull request #1069](https://github.com/ignitionrobotics/ign-gazebo/pull/1069) + * [Pull request #1023](https://github.com/ignitionrobotics/ign-gazebo/pull/1023) + +1. Suppress missing canonical link error messages for static models + * [Pull request #1068](https://github.com/ignitionrobotics/ign-gazebo/pull/1068) + +1. Heightmap fixes + * [Pull request #1055](https://github.com/ignitionrobotics/ign-gazebo/pull/1055) + * [Pull request #1054](https://github.com/ignitionrobotics/ign-gazebo/pull/1054) + +1. Place config files in a versioned directory + * [Pull request #1050](https://github.com/ignitionrobotics/ign-gazebo/pull/1050) + * [Pull request #1070](https://github.com/ignitionrobotics/ign-gazebo/pull/1070) + +1. Fix GUI crash when accessing bad rendering UserData + * [Pull request #1052](https://github.com/ignitionrobotics/ign-gazebo/pull/1052) + +1. Fix performance issue with contact data and AABB updates + * [Pull request #1048](https://github.com/ignitionrobotics/ign-gazebo/pull/1048) + +1. Enable new policy to fix protobuf compilation errors + * [Pull request #1046](https://github.com/ignitionrobotics/ign-gazebo/pull/1046) + +1. Support locked entities, and headless video recording using sim time + * [Pull request #862](https://github.com/ignitionrobotics/ign-gazebo/pull/862) + +1. Label Component & System, segmentation camera support + * [Pull request #853](https://github.com/ignitionrobotics/ign-gazebo/pull/853) + * [Pull request #1047](https://github.com/ignitionrobotics/ign-gazebo/pull/1047) + +1. Joint Force-Torque Systems Plugin + * [Pull request #977](https://github.com/ignitionrobotics/ign-gazebo/pull/977) + +1. Add support for cloning entities + * [Pull request #959](https://github.com/ignitionrobotics/ign-gazebo/pull/959) + +1. 🌐 Spherical coordinates + * [Pull request #1008](https://github.com/ignitionrobotics/ign-gazebo/pull/1008) + +1. Populate JointConstraintWrench from physics + * [Pull request #989](https://github.com/ignitionrobotics/ign-gazebo/pull/989) + +1. Buoyancy engine + * [Pull request #1009](https://github.com/ignitionrobotics/ign-gazebo/pull/1009) + +1. Infrastructure + * [Pull request #1033](https://github.com/ignitionrobotics/ign-gazebo/pull/1033) + * [Pull request #1029](https://github.com/ignitionrobotics/ign-gazebo/pull/1029) + * [Pull request #991](https://github.com/ignitionrobotics/ign-gazebo/pull/991) + * [Pull request #809](https://github.com/ignitionrobotics/ign-gazebo/pull/809) + +1. Update on resize instead of pre-render / render + * [Pull request #1028](https://github.com/ignitionrobotics/ign-gazebo/pull/1028) + +1. Add a flag to force headless rendering mode + * [Pull request #701](https://github.com/ignitionrobotics/ign-gazebo/pull/701) + +1. Remove unused ignition gui header + * [Pull request #1026](https://github.com/ignitionrobotics/ign-gazebo/pull/1026) + +1. Adds velocity control to JointPositionController. + * [Pull request #1003](https://github.com/ignitionrobotics/ign-gazebo/pull/1003) + +1. Collada world exporter now exporting lights + * [Pull request #912](https://github.com/ignitionrobotics/ign-gazebo/pull/912) + +1. Workaround for setting visual cast shadows without material + * [Pull request #1015](https://github.com/ignitionrobotics/ign-gazebo/pull/1015) + +1. Fix selection buffer crash on resize + * [Pull request #969](https://github.com/ignitionrobotics/ign-gazebo/pull/969) + +1. Remove extra xml version line in pendulum_links example world + * [Pull request #1002](https://github.com/ignitionrobotics/ign-gazebo/pull/1002) + +1. Enable sensor metrics on example worlds + * [Pull request #982](https://github.com/ignitionrobotics/ign-gazebo/pull/982) + +1. Add ESC to unselect entities in select entities plugin + * [Pull request #995](https://github.com/ignitionrobotics/ign-gazebo/pull/995) + +1. Visualize joints + * [Pull request #961](https://github.com/ignitionrobotics/ign-gazebo/pull/961) + +1. Deprecate particle emitter, and use scatter ratio in new particle mes… + * [Pull request #986](https://github.com/ignitionrobotics/ign-gazebo/pull/986) + +1. Removed unused variable in Shapes plugin + * [Pull request #984](https://github.com/ignitionrobotics/ign-gazebo/pull/984) + +1. Use root.Model() + * [Pull request #980](https://github.com/ignitionrobotics/ign-gazebo/pull/980) + +1. Add ModelSDF serializer + * [Pull request #851](https://github.com/ignitionrobotics/ign-gazebo/pull/851) + +1. Entity tree: prevent creation of repeated entity items + * [Pull request #974](https://github.com/ignitionrobotics/ign-gazebo/pull/974) + +1. Use statically-typed views for better performance + * [Pull request #856](https://github.com/ignitionrobotics/ign-gazebo/pull/856) + * [Pull request #1001](https://github.com/ignitionrobotics/ign-gazebo/pull/1001) + +1. Upgrade ign-sensors and support custom sensors + * [Pull request #617](https://github.com/ignitionrobotics/ign-gazebo/pull/617) + +1. Fix entity creation console msg + * [Pull request #972](https://github.com/ignitionrobotics/ign-gazebo/pull/972) + +1. Fix crash in the follow_actor example + * [Pull request #958](https://github.com/ignitionrobotics/ign-gazebo/pull/958) + +1. Removed pose topic from log system + * [Pull request #839](https://github.com/ignitionrobotics/ign-gazebo/pull/839) + +1. Be more specific when looking for physics plugins + * [Pull request #965](https://github.com/ignitionrobotics/ign-gazebo/pull/965) + +1. Complaint if Joint doesn't exists before adding joint controller + * [Pull request #786](https://github.com/ignitionrobotics/ign-gazebo/pull/786) + +1. [DiffDrive] add enable/disable + * [Pull request #772](https://github.com/ignitionrobotics/ign-gazebo/pull/772) + +1. Fix component inspector shutdown crash + * [Pull request #724](https://github.com/ignitionrobotics/ign-gazebo/pull/724) + +1. Setting the intiial velocity for a model or joint + * [Pull request #693](https://github.com/ignitionrobotics/ign-gazebo/pull/693) + +1. Examples and tutorial on using rendering API from plugins + * [Pull request #596](https://github.com/ignitionrobotics/ign-gazebo/pull/596) + +1. Add missing IGNITION_GAZEBO_VISIBLE macros + * [Pull request #563](https://github.com/ignitionrobotics/ign-gazebo/pull/563) + +1. Fix visibility macro names when used by a different component (Windows) + * [Pull request #564](https://github.com/ignitionrobotics/ign-gazebo/pull/564) + +1. No install apt recommends and clear cache + * [Pull request #423](https://github.com/ignitionrobotics/ign-gazebo/pull/423) + +1. Support adding systems that don't come from a plugin + * [Pull request #936](https://github.com/ignitionrobotics/ign-gazebo/pull/936) + +1. Fix tests that use multiple root level models or lights + * [Pull request #931](https://github.com/ignitionrobotics/ign-gazebo/pull/931) + +1. Make Gazebo aware of SetCameraPassCountPerGpuFlush + * [Pull request #921](https://github.com/ignitionrobotics/ign-gazebo/pull/921) + +1. Visualize center of mass + * [Pull request #903](https://github.com/ignitionrobotics/ign-gazebo/pull/903) + +1. Transparent mode + * [Pull request #878](https://github.com/ignitionrobotics/ign-gazebo/pull/878) + +1. Visualize inertia + * [Pull request #861](https://github.com/ignitionrobotics/ign-gazebo/pull/861) + +1. Remove deprecations: tock 🕑 + * [Pull request #875](https://github.com/ignitionrobotics/ign-gazebo/pull/875) + +1. Removed and moved tape measure and grid config to ign-gui + * [Pull request #870](https://github.com/ignitionrobotics/ign-gazebo/pull/870) + +1. Update wireframe visualization to support nested models + * [Pull request #832](https://github.com/ignitionrobotics/ign-gazebo/pull/832) + +1. Multi-LRAUV Swimming Race Example + * [Pull request #841](https://github.com/ignitionrobotics/ign-gazebo/pull/841) + +1. Add view control gui plugin and support orthographic view + * [Pull request #815](https://github.com/ignitionrobotics/ign-gazebo/pull/815) + +1. Wireframe mode + * [Pull request #816](https://github.com/ignitionrobotics/ign-gazebo/pull/816) + +1. Explain why detail::View symbols are visible + * [Pull request #788](https://github.com/ignitionrobotics/ign-gazebo/pull/788) + +1. Bump dependencies in fortress + * [Pull request #764](https://github.com/ignitionrobotics/ign-gazebo/pull/764) + ## Ignition Gazebo 5.x -### Ignition Gazebo 5.X.X (20XX-XX-XX) +### Ignition Gazebo 5.X.X (202X-XX-XX) + +### Ignition Gazebo 5.3.0 (2021-11-12) + +1. Prevent creation of spurious elements when saving worlds + * [Pull request #1192](https://github.com/ignitionrobotics/ign-gazebo/pull/1192) + +1. Added support for tracked vehicles + * [Pull request #869](https://github.com/ignitionrobotics/ign-gazebo/pull/869) + +1. Add components to dynamically set joint limits + * [Pull request #847](https://github.com/ignitionrobotics/ign-gazebo/pull/847) + +1. Fix updating component from state + * [Pull request #1181](https://github.com/ignitionrobotics/ign-gazebo/pull/1181) + +1. Extend odom publisher to allow 3D + * [Pull request #1180](https://github.com/ignitionrobotics/ign-gazebo/pull/1180) + +1. Fix updating a component's data via SerializedState msg + * [Pull request #1131](https://github.com/ignitionrobotics/ign-gazebo/pull/1131) + +1. Sensor systems work if loaded after sensors + * [Pull request #1104](https://github.com/ignitionrobotics/ign-gazebo/pull/1104) + +1. Fix generation of systems library symlinks in build directory + * [Pull request #1160](https://github.com/ignitionrobotics/ign-gazebo/pull/1160) + +1. Edit material colors in component inspector + * [Pull request #1123](https://github.com/ignitionrobotics/ign-gazebo/pull/1123) + +1. Support setting the background color for sensors + * [Pull request #1147](https://github.com/ignitionrobotics/ign-gazebo/pull/1147) + +1. Use `uint64_t` for ComponentInspector Entity IDs + * [Pull request #1144](https://github.com/ignitionrobotics/ign-gazebo/pull/1144) + +1. Fix integers and floats on component inspector + * [Pull request #1143](https://github.com/ignitionrobotics/ign-gazebo/pull/1143) + +### Ignition Gazebo 5.2.0 (2021-10-22) + +1. Fix performance level test flakiness + * [Pull request #1129](https://github.com/ignitionrobotics/ign-gazebo/pull/1129) + +1. Updates to camera video record from subt + * [Pull request #1117](https://github.com/ignitionrobotics/ign-gazebo/pull/1117) + +1. Better protect this->dataPtr->initialized with renderMutex. + * [Pull request #1119](https://github.com/ignitionrobotics/ign-gazebo/pull/1119) + +1. Use QTimer to update plugins in the Qt thread + * [Pull request #1095](https://github.com/ignitionrobotics/ign-gazebo/pull/1095) + +1. Adjust pose decimals based on element width + * [Pull request #1089](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. JointPositionController: Improve misleading error message + * [Pull request #1098](https://github.com/ignitionrobotics/ign-gazebo/pull/1098) + +1. Fixed IMU system plugin + * [Pull request #1043](https://github.com/ignitionrobotics/ign-gazebo/pull/1043) + +1. Cache top level and static to speed up physics system (Backport #656) + * [Pull request #993](https://github.com/ignitionrobotics/ign-gazebo/pull/993) + +1. Prevent crash and print error + * [Pull request #1099](https://github.com/ignitionrobotics/ign-gazebo/pull/1099) + +1. Performance: use std::unordered_map where possible in SceneManager + * [Pull request #1083](https://github.com/ignitionrobotics/ign-gazebo/pull/1083) + +1. Fix light control standalone example + * [Pull request #1077](https://github.com/ignitionrobotics/ign-gazebo/pull/1077) + +1. Parse new param for enabling / disabling IMU orientation output + * [Pull request #899](https://github.com/ignitionrobotics/ign-gazebo/pull/899) + +1. Enable new policy to fix protobuf compilation errors + * [Pull request #1059](https://github.com/ignitionrobotics/ign-gazebo/pull/1059) + +1. Fix performance issue with contact data and AABB updates + * [Pull request #1048](https://github.com/ignitionrobotics/ign-gazebo/pull/1048) + +1. Support locked entities, and headless video recording using sim time + * [Pull request #862](https://github.com/ignitionrobotics/ign-gazebo/pull/862) + +1. Update ign-gazebo4 changelog + * [Pull request #1031](https://github.com/ignitionrobotics/ign-gazebo/pull/1031) + +1. bump version and update changelog + * [Pull request #1029](https://github.com/ignitionrobotics/ign-gazebo/pull/1029) + +1. Remove unused ignition gui header + * [Pull request #1026](https://github.com/ignitionrobotics/ign-gazebo/pull/1026) + +1. Collada world exporter now exporting lights + * [Pull request #912](https://github.com/ignitionrobotics/ign-gazebo/pull/912) + +1. Fixed GUI's ComponentInspector light parameter + * [Pull request #1018](https://github.com/ignitionrobotics/ign-gazebo/pull/1018) + +1. Workaround for setting visual cast shadows without material + * [Pull request #1015](https://github.com/ignitionrobotics/ign-gazebo/pull/1015) + +1. Fix selection buffer crash on resize + * [Pull request #969](https://github.com/ignitionrobotics/ign-gazebo/pull/969) + +1. Update DART deps to local + * [Pull request #1005](https://github.com/ignitionrobotics/ign-gazebo/pull/1005) + +1. Remove extra xml version line in pendulum_links example world + * [Pull request #1002](https://github.com/ignitionrobotics/ign-gazebo/pull/1002) + +1. Enable sensor metrics on example worlds + * [Pull request #982](https://github.com/ignitionrobotics/ign-gazebo/pull/982) + +1. Make thermal sensor test more robust + * [Pull request #994](https://github.com/ignitionrobotics/ign-gazebo/pull/994) + +1. Improved doxygen + * [Pull request #996](https://github.com/ignitionrobotics/ign-gazebo/pull/996) + +1. Remove bitbucket-pipelines.yml + * [Pull request #991](https://github.com/ignitionrobotics/ign-gazebo/pull/991) + +1. Removed unused variable in Shapes plugin + * [Pull request #984](https://github.com/ignitionrobotics/ign-gazebo/pull/984) + +1. Entity tree: prevent creation of repeated entity items + * [Pull request #974](https://github.com/ignitionrobotics/ign-gazebo/pull/974) + +1. Updates when forward-porting to v4 + * [Pull request #973](https://github.com/ignitionrobotics/ign-gazebo/pull/973) + +1. Don't use $HOME on most tests (InternalFixture) + * [Pull request #971](https://github.com/ignitionrobotics/ign-gazebo/pull/971) + +1. Fix entity creation console msg + * [Pull request #972](https://github.com/ignitionrobotics/ign-gazebo/pull/972) + +1. Fix crash in the follow_actor example + * [Pull request #958](https://github.com/ignitionrobotics/ign-gazebo/pull/958) + +1. Be more specific when looking for physics plugins + * [Pull request #965](https://github.com/ignitionrobotics/ign-gazebo/pull/965) + +1. Drag and drop meshes into scene + * [Pull request #939](https://github.com/ignitionrobotics/ign-gazebo/pull/939) + +1. Allow referencing links in nested models in LiftDrag + * [Pull request #955](https://github.com/ignitionrobotics/ign-gazebo/pull/955) + +1. Complaint if Joint doesn't exists before adding joint controller + * [Pull request #786](https://github.com/ignitionrobotics/ign-gazebo/pull/786) + +1. Set protobuf_MODULE_COMPATIBLE before any find_package call + * [Pull request #957](https://github.com/ignitionrobotics/ign-gazebo/pull/957) + +1. DiffDrive add enable/disable + * [Pull request #772](https://github.com/ignitionrobotics/ign-gazebo/pull/772) + +1. Fix component inspector shutdown crash + * [Pull request #724](https://github.com/ignitionrobotics/ign-gazebo/pull/724) + +1. Add UserCommands Plugin. + * [Pull request #719](https://github.com/ignitionrobotics/ign-gazebo/pull/719) + +1. Expose a test fixture helper class + * [Pull request #926](https://github.com/ignitionrobotics/ign-gazebo/pull/926) + +1. Fix logic to disable server default plugins loading + * [Pull request #953](https://github.com/ignitionrobotics/ign-gazebo/pull/953) + +1. Porting Dome to Edifice: Windows, deprecations + * [Pull request #948](https://github.com/ignitionrobotics/ign-gazebo/pull/948) + +1. removed unneeded plugin update + * [Pull request #944](https://github.com/ignitionrobotics/ign-gazebo/pull/944) + +1. Functions to enable velocity and acceleration checks on Link + * [Pull request #935](https://github.com/ignitionrobotics/ign-gazebo/pull/935) + +1. Support adding systems that don't come from a plugin + * [Pull request #936](https://github.com/ignitionrobotics/ign-gazebo/pull/936) + +1. 3D plot GUI plugin + * [Pull request #917](https://github.com/ignitionrobotics/ign-gazebo/pull/917) + +1. 4 to 5 + * [Pull request #938](https://github.com/ignitionrobotics/ign-gazebo/pull/938) + +1. Fix joint controller without joint vel data + * [Pull request #937](https://github.com/ignitionrobotics/ign-gazebo/pull/937) + +1. 3 to 4 + * [Pull request #933](https://github.com/ignitionrobotics/ign-gazebo/pull/933) + +1. Model info CLI `ign model` + * [Pull request #893](https://github.com/ignitionrobotics/ign-gazebo/pull/893) + +1. Support Bullet on Edifice + * [Pull request #919](https://github.com/ignitionrobotics/ign-gazebo/pull/919) + +1. Don't create components for entities that don't exist + * [Pull request #927](https://github.com/ignitionrobotics/ign-gazebo/pull/927) + +1. Fix blender sdf export script and remove .material file from collada light export test + * [Pull request #923](https://github.com/ignitionrobotics/ign-gazebo/pull/923) + +1. Heightmap physics (with DART) + * [Pull request #661](https://github.com/ignitionrobotics/ign-gazebo/pull/661) + +1. Adds Mesh Tutorial + * [Pull request #915](https://github.com/ignitionrobotics/ign-gazebo/pull/915) + +1. 4 to 5 + * [Pull request #918](https://github.com/ignitionrobotics/ign-gazebo/pull/918) + +1. Fix updating GUI plugin on load + * [Pull request #904](https://github.com/ignitionrobotics/ign-gazebo/pull/904) + +1. 3 to 4 + * [Pull request #916](https://github.com/ignitionrobotics/ign-gazebo/pull/916) + +1. Physics system: update link poses if the canonical link pose has been updated + * [Pull request #876](https://github.com/ignitionrobotics/ign-gazebo/pull/876) + +1. Add blender sdf export tutorial + * [Pull request #895](https://github.com/ignitionrobotics/ign-gazebo/pull/895) + +1. Banana for Scale + * [Pull request #734](https://github.com/ignitionrobotics/ign-gazebo/pull/734) + +1. Fix textures not exporting after loading a world that uses obj models + * [Pull request #874](https://github.com/ignitionrobotics/ign-gazebo/pull/874) + +1. Fix documentation for the Sensor component + * [Pull request #898](https://github.com/ignitionrobotics/ign-gazebo/pull/898) + +1. Make depth camera tests more robust + * [Pull request #897](https://github.com/ignitionrobotics/ign-gazebo/pull/897) + +1. Use UINT64_MAX for kComponentTpyeIDInvalid instead of relying on underflow + * [Pull request #889](https://github.com/ignitionrobotics/ign-gazebo/pull/889) + +1. Fix mouse view control target position + * [Pull request #879](https://github.com/ignitionrobotics/ign-gazebo/pull/879) ### Ignition Gazebo 5.1.0 (2021-06-29) @@ -326,12 +939,47 @@ ## Ignition Gazebo 4.x -### Ignition Gazebo 4.11.x (2021-09-23) +### Ignition Gazebo 4.12.0 (2021-10-22) + +1. Fix performance issue with contact data and AABB updates. + * [Pull Request 1048](https://github.com/ignitionrobotics/ign-gazebo/pull/1048) + +1. Enable new CMake policy to fix protobuf compilation + * [Pull Request 1059](https://github.com/ignitionrobotics/ign-gazebo/pull/1059) + +1. Parse new param for enabling / disabling IMU orientation output. + * [Pull Request 899](https://github.com/ignitionrobotics/ign-gazebo/pull/899) + +1. Fix light control standalone example. + * [Pull Request 1077](https://github.com/ignitionrobotics/ign-gazebo/pull/1077) + +1. Performance: use std::unordered_map where possible in SceneManager. + * [Pull Request 1083](https://github.com/ignitionrobotics/ign-gazebo/pull/1083) + +1. Prevent crash when using workflow PBR material. + * [Pull Request 1099](https://github.com/ignitionrobotics/ign-gazebo/pull/1099) + +1. JointPositionController: Improve misleading error message. + * [Pull Request 1098](https://github.com/ignitionrobotics/ign-gazebo/pull/1098) + +1. Adjust pose decimals based on element width. + * [Pull Request 1089](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. Better protect this->dataPtr->initialized with renderMutex. + * [Pull Request 1119](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. Updates to camera video record from subt. + * [Pull Request 1117](https://github.com/ignitionrobotics/ign-gazebo/pull/1117) + +1. Fix performance level test flakiness. + * [Pull Request 1129](https://github.com/ignitionrobotics/ign-gazebo/pull/1129) + +### Ignition Gazebo 4.11.0 (2021-09-23) 1. Support locked entities, and headless video recording using sim time. * [Pull Request 862](https://github.com/ignitionrobotics/ign-gazebo/pull/862) -### Ignition Gazebo 4.10.x (2021-09-15) +### Ignition Gazebo 4.10.0 (2021-09-15) 1. Fixed GUI's ComponentInspector light parameter * [Pull Request 1018](https://github.com/ignitionrobotics/ign-gazebo/pull/1018) @@ -815,8 +1463,78 @@ ## Ignition Gazebo 3.x -### Ignition Gazebo 3.X.X (202X-XX-XX) +### Ignition Gazebo 3.X.X (20XX-XX-XX) + +### Ignition Gazebo 3.12.0 (2021-11-11) + +1. Prevent creation of spurious `` elements when saving worlds + * [Pull request #1192](https://github.com/ignitionrobotics/ign-gazebo/pull/1192) + +1. Added support for tracked vehicles + * [Pull request #869](https://github.com/ignitionrobotics/ign-gazebo/pull/869) + +1. Add components to dynamically set joint limits + * [Pull request #847](https://github.com/ignitionrobotics/ign-gazebo/pull/847) + +1. Fix updating a component's data via SerializedState msg + * [Pull request #1149](https://github.com/ignitionrobotics/ign-gazebo/pull/1149) + +1. Sensor systems work if loaded after sensors + * [Pull request #1104](https://github.com/ignitionrobotics/ign-gazebo/pull/1104) + +1. Fix generation of systems library symlinks in build directory + * [Pull request #1160](https://github.com/ignitionrobotics/ign-gazebo/pull/1160) + +1. Backport gazebo::Util::validTopic() from ign-gazebo4. + * [Pull request #1153](https://github.com/ignitionrobotics/ign-gazebo/pull/1153) + +1. Support setting the background color for sensors + * [Pull request #1147](https://github.com/ignitionrobotics/ign-gazebo/pull/1147) + +1. Use uint64_t for ComponentInspector Entity IDs + * [Pull request #1144](https://github.com/ignitionrobotics/ign-gazebo/pull/1144) + +1. Fix integers and floats on component inspector + * [Pull request #1143](https://github.com/ignitionrobotics/ign-gazebo/pull/1143) + +### Ignition Gazebo 3.11.0 (2021-10-21) + +1. Updates to camera video record from subt. + * [Pull request #1117](https://github.com/ignitionrobotics/ign-gazebo/pull/1117) +1. Fix performance level test flakiness. + * [Pull request #1129](https://github.com/ignitionrobotics/ign-gazebo/pull/1129) + +### Ignition Gazebo 3.10.0 (2021-10-15) + +1. Performance: use std::unordered_map where possible in SceneManager + * [Pull request #1083](https://github.com/ignitionrobotics/ign-gazebo/pull/1083) + +1. Enable new CMake policy to fix protobuf compilation + * [Pull request #1059](https://github.com/ignitionrobotics/ign-gazebo/pull/1059) + +1. Fix setting cast_shadows for visuals without material + * [Pull request #1015](https://github.com/ignitionrobotics/ign-gazebo/pull/1015) + +1. Remove duplicate XML tag in pendulum_links example world + * [Pull request #1002](https://github.com/ignitionrobotics/ign-gazebo/pull/1002) + +1. Enable sensor metrics on example worlds + * [Pull request #982](https://github.com/ignitionrobotics/ign-gazebo/pull/982) + +1. Improved doxygen + * [Pull request #996](https://github.com/ignitionrobotics/ign-gazebo/pull/996) + +1. JointPositionController: Improve misleading error message + * [Pull request #1098](https://github.com/ignitionrobotics/ign-gazebo/pull/1098) + +1. Adjust pose decimals based on element width + * [Pull request #1089](https://github.com/ignitionrobotics/ign-gazebo/pull/1089) + +1. Fixed IMU system plugin + * [Pull request #1043](https://github.com/ignitionrobotics/ign-gazebo/pull/1043) +1. Use QTimer to update plugins in the Qt thread + * [Pull request #1095](https://github.com/ignitionrobotics/ign-gazebo/pull/1095) ### Ignition Gazebo 3.9.0 (2021-08-16) diff --git a/Migration.md b/Migration.md index cad3dd15486..4fbe67cda46 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,79 @@ Deprecated code produces compile-time warnings. These warning serve as notification to users that their code should be upgraded. The next major release will remove the deprecated code. +## Ignition Gazebo 6.1 to 6.2 + +* If no `` is given to the `Thruster` plugin, the namespace now + defaults to the model name, instead of an empty string. + +## Ignition Gazebo 5.x to 6.x + +* The ParticleEmitter system is deprecated. Please use the ParticleEmitter2 +system. + +* Marker example has been moved to Ignition GUI. + +* Some GUI plugins have been moved to Ignition GUI. Gazebo users don't need to + change their configuration files, the plugins will be loaded the same way. + * Grid Config + * Tape Measure + +* `dynamic_pose/info` topic is removed from `LogRecord` and `LogPlayback` +since pose information is being logged in the `changed_state` topic. + +* The internal management of entities and components in the + `EntityComponentManager` has been updated to improve runtime performance. As a + result, several methods have been deprecated, and a few types have changed. + * **Deprecated**: + + All `EntityComponentManager` methods that use `ComponentKey` as an input + parameter. + + The `EntityComponentManager::First` method. + + The `ComponentId` and `ComponentKey` types are now deprecated. A + combination of `Entity` and `ComponentTypeId` should be used instead. + + The `components::StorageDescriptorBase` and + `components::StorageDescriptor` classes. + + Methods in `components::Factory` that have deprecated input parameter + types and/or deprecated return types. + - The version of `components::Factory::Register` which has a + `StorageDescriptorBase *` input parameter. + - `components::Factory::NewStorage` + + The `ComponentStorageBase` and `ComponentStorage` classes. + * **Modified**: + + `EntityComponentManager::CreateComponent` now returns a pointer to the + created component instead of a `ComponentKey`. + + `ComponentKey` has been modified to be a + `std::pair` (it used to be a + `std::pair`) since the `ComponentId` type + is now deprecated. `ComponentKey` has also been deprecated, so usage of + this type is discouraged (see the **Deprecated** section above for more + information about how to replace usage of `ComponentKey`). + +* The `GzScene3D` GUI plugin is being deprecated in favor of `MinimalScene`. In + order to get the same functionality as `GzScene3D`, users need to add the + following plugins: + + `MinimalScene`: base rendering functionality + + `GzSceneManager`: adds / removes / moves entities in the scene + + `EntityContextMenuPlugin`: right-click menu + + `InteractiveViewControl`: orbit controls + + `CameraTracking`: Move to, follow, set camera pose + + `MarkerManager`: Enables the use of markers + + `SelectEntities`: Select entities clicking on the scene + + `Spawn`: Functionality to spawn entities into the scene via GUI + + `VisualizationCapabilities`: View collisions, inertial, CoM, joints, etc. + + Moreover, legacy mode needs to be turned off for the following plugins + for them to work with `MinimalScene` (set `false`): + + `TransformControl`: Translate and rotate + + `ViewAndle`: Move camera to preset angles + +* The `gui.config` and `server.config` files are now located in a versioned + folder inside `$HOME/.ignition/gazebo`, i.e. `$HOME/.ignition/gazebo/6/gui.config`. + +## Ignition Gazebo 5.2 to 5.3 + +* If no `` is given to the `Thruster` plugin, the namespace now + defaults to the model name, instead of an empty string. + ## Ignition Gazebo 4.x to 5.x * Use `cli` component of `ignition-utils1`. diff --git a/README.md b/README.md index 090d774df9f..d5b19186a9c 100644 --- a/README.md +++ b/README.md @@ -9,10 +9,10 @@ Build | Status -- | -- -Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-gazebo/branch/main/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-gazebo) -Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-main-bionic-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-main-bionic-amd64) -Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-main-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-main-homebrew-amd64) -Windows | [![Build Status](https://build.osrfoundation.org/job/ign_gazebo-ci-win/badge/icon)](https://build.osrfoundation.org/job/ign_gazebo-ci-win/) +Test coverage | [![codecov](https://codecov.io/gh/ignitionrobotics/ign-gazebo/branch/ign-gazebo6/graph/badge.svg)](https://codecov.io/gh/ignitionrobotics/ign-gazebo) +Ubuntu Bionic | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-ign-gazebo6-bionic-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-ign-gazebo6-bionic-amd64) +Homebrew | [![Build Status](https://build.osrfoundation.org/buildStatus/icon?job=ignition_gazebo-ci-ign-gazebo6-homebrew-amd64)](https://build.osrfoundation.org/job/ignition_gazebo-ci-ign-gazebo6-homebrew-amd64) +Windows | [![Build Status](https://build.osrfoundation.org/job/ign_gazebo-ign-6-win/badge/icon)](https://build.osrfoundation.org/job/ign_gazebo-ign-6-win/) Ignition Gazebo is an open source robotics simulator. Through Ignition Gazebo, users have access to high fidelity physics, rendering, and sensor models. Additionally, users and developers have multiple points of entry to simulation including a graphical user interface, plugins, and asynchronous message passing and services. @@ -24,14 +24,6 @@ Ignition Gazebo is derived from [Gazebo](http://gazebosim.org) and represents ov [Install](#install) -* [Binary Install](#binary-install) - -* [Source Install](#source-install) - - * [Prerequisites](#prerequisites) - - * [Building from Source](#building-from-source) - [Usage](#usage) [Documentation](#documentation) @@ -86,120 +78,7 @@ introspection and control. # Install -We recommend following the [Binary Install](#binary-install) instructions to get up and running as quickly and painlessly as possible. - -The [Source Install](#source-install) instructions should be used if you need the very latest software improvements, you need to modify the code, or you plan to make a contribution. - -## Binary Install - -The binary install method will use pre-built packages which are typically -available through a package management utility such as [Apt](https://wiki.debian.org/Apt). -This approach eliminates the need to download and compile source code, and dependencies -are handled for you. The downside of a binary install is that you won't be able to modify -the code. See [Source Install](#source-install) for information on -installing Ignition Gazebo from source. - -**Ubuntu Bionic** - -1. Configure package repositories. - - ``` - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - ``` - - ``` - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' - ``` - - ``` - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - - ``` - - ``` - sudo apt-get update - ``` - -2. Install Ignition Gazebo - - ``` - sudo apt-get install libignition-gazebo<#>-dev - ``` - - Where `<#>` is the desired version number, like 3 or 4. - -## Source Install - -Install from source if you're interested in changing the source code or need a -feature which hasn't been released yet. - -### Prerequisites - -Ignition Gazebo has a fairly large set of dependencies. Refer to the following sections -for dependency installation instructions for each supported operating system. - -**[Ubuntu Bionic](http://releases.ubuntu.com/18.04/)** - -1. Enable the Ignition software repositories: - - ``` - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' - ``` - - ``` - sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-prerelease `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-prerelease.list' - ``` - - ``` - wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - - ``` - - ``` - sudo apt-get update - ``` - -2. Install package dependencies: - - ``` - git clone https://github.com/ignitionrobotics/ign-gazebo -b main - ``` - - ``` - export SYSTEM_VERSION=bionic - sudo apt -y install \ - $(sort -u $(find . -iname 'packages-'$SYSTEM_VERSION'.apt' -o -iname 'packages.apt') | tr '\n' ' ') - ``` - -### Building from source - -1. Install [prerequisites](#prerequisites) - -2. Configure gcc8 - - * Ubuntu - - ``` - sudo apt-get install g++-8 - ``` - - ``` - sudo update-alternatives --install /usr/bin/gcc gcc /usr/bin/gcc-8 800 --slave /usr/bin/g++ g++ /usr/bin/g++-8 --slave /usr/bin/gcov gcov /usr/bin/gcov-8 - ``` - -3. Clone the repository if you haven't already. - - ``` - git clone https://github.com/ignitionrobotics/ign-gazebo -b main - ``` - -4. Configure and build. - - ``` - cd ign-gazebo - mkdir build - cd build - cmake ../ - make - ``` +See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.1/install.html). # Usage @@ -235,70 +114,22 @@ line is using symbolic links to each library's YAML file. ``` mkdir ~/.ignition/tools/configs -p cd ~/.ignition/tools/configs/ -ln -s /usr/local/share/ignition/fuel6.yaml . -ln -s /usr/local/share/ignition/transport10.yaml . -ln -s /usr/local/share/ignition/transportlog10.yaml . +ln -s /usr/local/share/ignition/fuel7.yaml . +ln -s /usr/local/share/ignition/transport11.yaml . +ln -s /usr/local/share/ignition/transportlog11.yaml . ... export IGN_CONFIG_PATH=$HOME/.ignition/tools/configs ``` This issue is tracked [here](https://github.com/ignitionrobotics/ign-tools/issues/8). -# Documentation - -API documentation and tutorials can be accessed at [https://ignitionrobotics.org/libs/gazebo](https://ignitionrobotics.org/libs/gazebo) - -You can also generate the documentation from a clone of this repository by following these steps. +# Documentation -1. You will need [Doxygen](http://www.doxygen.org/). On Ubuntu Doxygen can be installed using - - ``` - sudo apt-get install doxygen - ``` - -2. Clone the repository - - ``` - git clone https://github.com/ignitionrobotics/ign-gazebo - ``` - -3. Configure and build the documentation. - - ``` - cd ign-gazebo - mkdir build - cd build - cmake ../ - make doc - ``` - -4. View the documentation by running the following command from the `build` directory. - - ``` - firefox doxygen/html/index.html - ``` +See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.0/install.html). # Testing -Follow these steps to run tests and static code analysis in your clone of this repository. - -1. Follow the [source install instructions](#source-install). - -2. Run tests. - - ``` - make test - ``` - -3. Static code checker. - - ``` - sudo apt-get update && sudo apt-get -y install cppcheck - ``` - - ``` - make codecheck - ``` +See the [installation tutorial](https://ignitionrobotics.org/api/gazebo/6.0/install.html). See the [Writing Tests section of the contributor guide](https://github.com/ignitionrobotics/ign-gazebo/blob/main/CONTRIBUTING.md#writing-tests) for help creating or modifying tests. diff --git a/docker/Dockerfile.nightly b/docker/Dockerfile.nightly index b862b29d0f1..a22b5d708b7 100644 --- a/docker/Dockerfile.nightly +++ b/docker/Dockerfile.nightly @@ -13,17 +13,17 @@ RUN apt-get update \ && apt-get install -y \ libignition-cmake2-dev \ libignition-common4-dev \ - libignition-fuel-tools6-dev \ + libignition-fuel-tools7-dev \ libignition-math6-eigen3-dev \ libignition-plugin-dev \ - libignition-physics4-dev \ - libignition-rendering5-dev \ + libignition-physics5-dev \ + libignition-rendering6-dev \ libignition-tools-dev \ - libignition-transport10-dev \ - libignition-gui5-dev \ - libignition-msgs7-dev \ - libignition-sensors5-dev \ - libsdformat11-dev + libignition-transport11-dev \ + libignition-gui6-dev \ + libignition-msgs8-dev \ + libignition-sensors6-dev \ + libsdformat12-dev COPY . ign-gazebo RUN cd ign-gazebo \ diff --git a/examples/plugin/command_actor/CMakeLists.txt b/examples/plugin/command_actor/CMakeLists.txt index ccc5c9cfc54..9d52f5539d5 100644 --- a/examples/plugin/command_actor/CMakeLists.txt +++ b/examples/plugin/command_actor/CMakeLists.txt @@ -7,9 +7,9 @@ project(CommandActor) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -find_package(ignition-gazebo5 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) add_library(CommandActor SHARED CommandActor.cc) set_property(TARGET CommandActor PROPERTY CXX_STANDARD 17) target_link_libraries(CommandActor PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - PRIVATE ignition-gazebo5::ignition-gazebo5) + PRIVATE ignition-gazebo6::ignition-gazebo6) diff --git a/examples/plugin/command_actor/command_actor.sdf b/examples/plugin/command_actor/command_actor.sdf index 390a8e86a0b..e091290e95d 100644 --- a/examples/plugin/command_actor/command_actor.sdf +++ b/examples/plugin/command_actor/command_actor.sdf @@ -10,74 +10,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/actors/control - /world/actors/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/actors/stats - - - - - true 0 0 10 0 0 0 diff --git a/examples/plugin/custom_component/CMakeLists.txt b/examples/plugin/custom_component/CMakeLists.txt index 72eeec354f3..391bc7c897b 100644 --- a/examples/plugin/custom_component/CMakeLists.txt +++ b/examples/plugin/custom_component/CMakeLists.txt @@ -7,11 +7,11 @@ project(CustomComponentPlugin) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -find_package(ignition-gazebo5 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) add_library(CustomComponentPlugin SHARED CustomComponentPlugin.cc ) set_property(TARGET CustomComponentPlugin PROPERTY CXX_STANDARD 17) target_link_libraries(CustomComponentPlugin PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - PRIVATE ignition-gazebo5::ignition-gazebo5) + PRIVATE ignition-gazebo6::ignition-gazebo6) diff --git a/examples/plugin/custom_sensor_system/CMakeLists.txt b/examples/plugin/custom_sensor_system/CMakeLists.txt new file mode 100644 index 00000000000..78fa46f9410 --- /dev/null +++ b/examples/plugin/custom_sensor_system/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) + +find_package(ignition-cmake2 REQUIRED) + +project(OdometerSystem) + +ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) + +ign_find_package(ignition-gazebo6 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + +find_package(ignition-sensors6 REQUIRED) +set(IGN_SENSORS_VER ${ignition-sensors6_VERSION_MAJOR}) + +# Fetch the custom sensor example from ign-sensors +# Users won't commonly use this to fetch their sensors. The sensor may be part +# of the system's CMake project, or installed from another project, etc... +include(FetchContent) +FetchContent_Declare( + sensors_clone + GIT_REPOSITORY https://github.com/ignitionrobotics/ign-sensors + GIT_TAG ign-sensors6 +) +FetchContent_Populate(sensors_clone) +add_subdirectory(${sensors_clone_SOURCE_DIR}/examples/custom_sensor ${sensors_clone_BINARY_DIR}) + +add_library(${PROJECT_NAME} SHARED ${PROJECT_NAME}.cc) +target_link_libraries(${PROJECT_NAME} + PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + PRIVATE ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER} + PRIVATE ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} + PRIVATE odometer +) +target_include_directories(${PROJECT_NAME} + PUBLIC ${sensors_clone_SOURCE_DIR}/examples/custom_sensor) diff --git a/examples/plugin/custom_sensor_system/OdometerSystem.cc b/examples/plugin/custom_sensor_system/OdometerSystem.cc new file mode 100644 index 00000000000..3da7b2b3353 --- /dev/null +++ b/examples/plugin/custom_sensor_system/OdometerSystem.cc @@ -0,0 +1,132 @@ +/* + * 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 "Odometer.hh" +#include "OdometerSystem.hh" + +using namespace custom; + +////////////////////////////////////////////////// +void OdometerSystem::PreUpdate(const ignition::gazebo::UpdateInfo &, + ignition::gazebo::EntityComponentManager &_ecm) +{ + _ecm.EachNew( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::CustomSensor *_custom, + const ignition::gazebo::components::ParentEntity *_parent)->bool + { + // Get sensor's scoped name without the world + auto sensorScopedName = ignition::gazebo::removeParentScope( + ignition::gazebo::scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _custom->Data(); + data.SetName(sensorScopedName); + + // Default to scoped name as topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/odometer"; + data.SetTopic(topic); + } + + ignition::sensors::SensorFactory sensorFactory; + auto sensor = sensorFactory.CreateSensor(data); + if (nullptr == sensor) + { + ignerr << "Failed to create odometer [" << sensorScopedName << "]" + << std::endl; + return false; + } + + // Set sensor parent + auto parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // Set topic on Gazebo + _ecm.CreateComponent(_entity, + ignition::gazebo::components::SensorTopic(sensor->Topic())); + + // Keep track of this sensor + this->entitySensorMap.insert(std::make_pair(_entity, + std::move(sensor))); + + return true; + }); +} + +////////////////////////////////////////////////// +void OdometerSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + // Only update and publish if not paused. + if (!_info.paused) + { + for (auto &[entity, sensor] : this->entitySensorMap) + { + sensor->NewPosition(ignition::gazebo::worldPose(entity, _ecm).Pos()); + sensor->Update(_info.simTime); + } + } + + this->RemoveSensorEntities(_ecm); +} + +////////////////////////////////////////////////// +void OdometerSystem::RemoveSensorEntities( + const ignition::gazebo::EntityComponentManager &_ecm) +{ + _ecm.EachRemoved( + [&](const ignition::gazebo::Entity &_entity, + const ignition::gazebo::components::CustomSensor *)->bool + { + if (this->entitySensorMap.erase(_entity) == 0) + { + ignerr << "Internal error, missing odometer for entity [" + << _entity << "]" << std::endl; + } + return true; + }); +} + +IGNITION_ADD_PLUGIN(OdometerSystem, ignition::gazebo::System, + OdometerSystem::ISystemPreUpdate, + OdometerSystem::ISystemPostUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS(OdometerSystem, "custom::OdometerSystem") diff --git a/examples/plugin/custom_sensor_system/OdometerSystem.hh b/examples/plugin/custom_sensor_system/OdometerSystem.hh new file mode 100644 index 00000000000..63bdcec04b8 --- /dev/null +++ b/examples/plugin/custom_sensor_system/OdometerSystem.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 ODOMETERSYSTEM_HH_ +#define ODOMETERSYSTEM_HH_ + +#include +#include +#include + +namespace custom +{ + /// \brief Example showing how to tie a custom sensor, in this case an + /// odometer, into simulation + class OdometerSystem: + public ignition::gazebo::System, + public ignition::gazebo::ISystemPreUpdate, + public ignition::gazebo::ISystemPostUpdate + { + // Documentation inherited. + // During PreUpdate, check for new sensors that were inserted + // into simulation and create more components as needed. + public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) final; + + // Documentation inherited. + // During PostUpdate, update the known sensors and publish their data. + // Also remove sensors that have been deleted. + public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) final; + + /// \brief Remove custom sensors if their entities have been removed from + /// simulation. + /// \param[in] _ecm Immutable reference to ECM. + private: void RemoveSensorEntities( + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief A map of custom entities to their sensors + private: std::unordered_map> entitySensorMap; + }; +} +#endif diff --git a/examples/plugin/custom_sensor_system/README.md b/examples/plugin/custom_sensor_system/README.md new file mode 100644 index 00000000000..7145c7b91c5 --- /dev/null +++ b/examples/plugin/custom_sensor_system/README.md @@ -0,0 +1,45 @@ +# Custom sensor system + +This example shows how to use a custom sensor with Ignition Gazebo. + +It uses the odometer created on this example: +[ign-sensors/examples/custom_sensor](https://github.com/ignitionrobotics/ign-sensors/tree/main/examples/custom_sensor). + +## Build + +From the root of the `ign-gazebo` repository, do the following to build the example: + +~~~ +cd examples/plugins/custom_sensor_system +mkdir build +cd build +cmake .. +make +~~~ + +This will generate the `OdometerSystem` library under `build`. + +## Run + +The plugin must be attached to an entity to be loaded. This is demonstrated in +the `odometer.sdf` file that's going to be loaded. + +Before starting Gazebo, we must make sure it can find the plugin by doing: + +~~~ +cd examples/plugins/custom_sensor_system +export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/build +~~~ + +Then load the example world: + + ign gazebo -r odometer.sdf + +You should see a box slowly moving in a straight line. + +Listen to the odometer data with: + +``` +ign topic -e -t /world/odometer_world/model/model_with_sensor/link/link/sensor/an_odometer/odometer +``` + diff --git a/examples/plugin/custom_sensor_system/odometer.sdf b/examples/plugin/custom_sensor_system/odometer.sdf new file mode 100644 index 00000000000..f9c7817b296 --- /dev/null +++ b/examples/plugin/custom_sensor_system/odometer.sdf @@ -0,0 +1,111 @@ + + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.05 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1 + 30 + true + + + 0.00001 + 0.00001 + + + + + + + + 0.2 0 0 + + + + + diff --git a/examples/plugin/gui_system_plugin/CMakeLists.txt b/examples/plugin/gui_system_plugin/CMakeLists.txt index 1d2a11bb2ae..8c80ddd6fa9 100644 --- a/examples/plugin/gui_system_plugin/CMakeLists.txt +++ b/examples/plugin/gui_system_plugin/CMakeLists.txt @@ -8,7 +8,7 @@ project(GuiSystemPlugin) set(CMAKE_AUTOMOC ON) -find_package(ignition-gazebo5 REQUIRED COMPONENTS gui) +find_package(ignition-gazebo6 REQUIRED COMPONENTS gui) QT5_ADD_RESOURCES(resources_RCC ${PROJECT_NAME}.qrc) @@ -17,5 +17,5 @@ add_library(${PROJECT_NAME} SHARED ${resources_RCC} ) target_link_libraries(${PROJECT_NAME} - PRIVATE ignition-gazebo5::gui + PRIVATE ignition-gazebo6::gui ) diff --git a/examples/plugin/hello_world/CMakeLists.txt b/examples/plugin/hello_world/CMakeLists.txt index 6d6213c2c7e..eec9865537e 100644 --- a/examples/plugin/hello_world/CMakeLists.txt +++ b/examples/plugin/hello_world/CMakeLists.txt @@ -7,8 +7,8 @@ project(Hello_world) ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -ign_find_package(ignition-gazebo5 REQUIRED) -set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) +ign_find_package(ignition-gazebo6 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) add_library(HelloWorld SHARED HelloWorld) set_property(TARGET HelloWorld PROPERTY CXX_STANDARD 17) diff --git a/examples/plugin/rendering_plugins/CMakeLists.txt b/examples/plugin/rendering_plugins/CMakeLists.txt index 73a9a9c12f2..46de43ab5a0 100644 --- a/examples/plugin/rendering_plugins/CMakeLists.txt +++ b/examples/plugin/rendering_plugins/CMakeLists.txt @@ -7,14 +7,14 @@ endif() project(RenderingPlugins) # Common to both plugins -find_package(ignition-rendering5 REQUIRED) +find_package(ignition-rendering6 REQUIRED) # GUI plugin set(GUI_PLUGIN RenderingGuiPlugin) set(CMAKE_AUTOMOC ON) -find_package(ignition-gui5 REQUIRED) +find_package(ignition-gui6 REQUIRED) QT5_ADD_RESOURCES(resources_RCC ${GUI_PLUGIN}.qrc) @@ -24,21 +24,21 @@ add_library(${GUI_PLUGIN} SHARED ) target_link_libraries(${GUI_PLUGIN} PRIVATE - ignition-gui5::ignition-gui5 - ignition-rendering5::ignition-rendering5 + ignition-gui6::ignition-gui6 + ignition-rendering6::ignition-rendering6 ) # Server plugin set(SERVER_PLUGIN RenderingServerPlugin) find_package(ignition-plugin1 REQUIRED COMPONENTS register) -find_package(ignition-gazebo5 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) add_library(${SERVER_PLUGIN} SHARED ${SERVER_PLUGIN}.cc) set_property(TARGET ${SERVER_PLUGIN} PROPERTY CXX_STANDARD 17) target_link_libraries(${SERVER_PLUGIN} PRIVATE ignition-plugin1::ignition-plugin1 - ignition-gazebo5::ignition-gazebo5 - ignition-rendering5::ignition-rendering5 + ignition-gazebo6::ignition-gazebo6 + ignition-rendering6::ignition-rendering6 ) diff --git a/examples/plugin/rendering_plugins/rendering_plugins.sdf b/examples/plugin/rendering_plugins/rendering_plugins.sdf index 8fbb572bda1..68f77eb736a 100644 --- a/examples/plugin/rendering_plugins/rendering_plugins.sdf +++ b/examples/plugin/rendering_plugins/rendering_plugins.sdf @@ -17,7 +17,7 @@ - + 3D View false @@ -31,6 +31,47 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + @@ -51,6 +92,7 @@ true true true + true diff --git a/examples/plugin/system_plugin/CMakeLists.txt b/examples/plugin/system_plugin/CMakeLists.txt index 6fcc9de2622..bc30c2d7efc 100644 --- a/examples/plugin/system_plugin/CMakeLists.txt +++ b/examples/plugin/system_plugin/CMakeLists.txt @@ -7,9 +7,9 @@ project(SampleSystem) find_package(ignition-plugin1 REQUIRED COMPONENTS register) set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) -find_package(ignition-gazebo5 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) add_library(SampleSystem SHARED SampleSystem.cc SampleSystem2.cc) set_property(TARGET SampleSystem PROPERTY CXX_STANDARD 17) target_link_libraries(SampleSystem PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} - PRIVATE ignition-gazebo5::ignition-gazebo5) + PRIVATE ignition-gazebo6::ignition-gazebo6) diff --git a/examples/scripts/distributed/primary.sdf b/examples/scripts/distributed/primary.sdf index 62961b30086..20644d3006e 100644 --- a/examples/scripts/distributed/primary.sdf +++ b/examples/scripts/distributed/primary.sdf @@ -6,75 +6,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/default/control - /world/default/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/default/stats - - - - - 0.8 0.8 0.8 1.0 0.34 0.39 0.43 1.0 diff --git a/examples/scripts/distributed/primary.sh b/examples/scripts/distributed/primary.sh index 3bcd6d9060b..d4c138ac6b9 100755 --- a/examples/scripts/distributed/primary.sh +++ b/examples/scripts/distributed/primary.sh @@ -4,12 +4,3 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" ign gazebo -v 4 -z 100000000 --network-role primary --network-secondaries 3 $DIR/primary.sdf -# Ignition Gazebo 1.x.x and 2.x.x support using environment variables to -# configure distributed simulation. This capability is deprecated in -# version 2.x.x, and removed in verion 3.x.x of Inition Gazebo. Please use the -# --network-role and --network-secondaries command line options instead. - -# export IGN_GAZEBO_NETWORK_ROLE="PRIMARY" -# export IGN_GAZEBO_NETWORK_SECONDARIES=3 -# ign-gazebo -v 4 --distributed -f $DIR/primary.sdf - diff --git a/examples/scripts/distributed/secondary.sh b/examples/scripts/distributed/secondary.sh index cb60cf7c617..c3b1587981c 100755 --- a/examples/scripts/distributed/secondary.sh +++ b/examples/scripts/distributed/secondary.sh @@ -4,10 +4,3 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" ign gazebo -s -v 4 -z 100000000 --network-role secondary $DIR/secondary.sdf -# Ignition Gazebo 1.x.x and 2.x.x support using environment variables to -# configure distributed simulation. This capability is deprecated in -# version 2.x.x, and removed in verion 3.x.x of Inition Gazebo. Please use the -# --network-role and --network-secondaries command line options instead. - -# export IGN_GAZEBO_NETWORK_ROLE="SECONDARY" -# ign-gazebo -v 4 --distributed -f $DIR/secondary.sdf diff --git a/examples/scripts/distributed/standalone.sdf b/examples/scripts/distributed/standalone.sdf index 1b952379003..6bdac8c0c49 100644 --- a/examples/scripts/distributed/standalone.sdf +++ b/examples/scripts/distributed/standalone.sdf @@ -10,75 +10,6 @@ name="ignition::gazebo::systems::Physics"> - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/default/control - /world/default/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/default/stats - - - - - 0.8 0.8 0.8 1.0 0.34 0.39 0.43 1.0 diff --git a/examples/scripts/distributed_levels/distributed_levels.sdf.erb b/examples/scripts/distributed_levels/distributed_levels.sdf.erb index 1b0b16e663d..b3f03c56edc 100644 --- a/examples/scripts/distributed_levels/distributed_levels.sdf.erb +++ b/examples/scripts/distributed_levels/distributed_levels.sdf.erb @@ -113,75 +113,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/default/control - /world/default/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/default/stats - - - - <% end %> diff --git a/examples/scripts/distributed_levels/primary.sdf b/examples/scripts/distributed_levels/primary.sdf index 9e79e592aff..b9074299cfe 100644 --- a/examples/scripts/distributed_levels/primary.sdf +++ b/examples/scripts/distributed_levels/primary.sdf @@ -10,84 +10,11 @@ - - - - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/default/control - /world/default/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/default/stats - - - - - - 0.8 0.8 0.8 1.0 0.34 0.39 0.43 1.0 diff --git a/examples/scripts/distributed_levels/primary.sh b/examples/scripts/distributed_levels/primary.sh index 0b0d231981e..e402340b408 100755 --- a/examples/scripts/distributed_levels/primary.sh +++ b/examples/scripts/distributed_levels/primary.sh @@ -5,13 +5,3 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" # --levels is implied by --network-role ign gazebo -v 4 -z 100000000 --network-role primary --network-secondaries 2 $DIR/primary.sdf -# Ignition Gazebo 1.x.x and 2.x.x support using environment variables to -# configure distributed simulation. This capability is deprecated in -# version 2.x.x, and removed in verion 3.x.x of Inition Gazebo. Please use the -# --network-role and --network-secondaries command line options instead. - -# export IGN_GAZEBO_NETWORK_ROLE="PRIMARY" -# export IGN_GAZEBO_NETWORK_SECONDARIES=3 -# ign-gazebo -v 4 --distributed -f $DIR/primary.sdf - - diff --git a/examples/scripts/distributed_levels/secondary.sh b/examples/scripts/distributed_levels/secondary.sh index f1ba79cf3ee..fb98c7a19f8 100755 --- a/examples/scripts/distributed_levels/secondary.sh +++ b/examples/scripts/distributed_levels/secondary.sh @@ -5,10 +5,3 @@ DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" # --levels is implied by --network-role ign gazebo -s -v 4 -z 100000000 --network-role secondary $DIR/secondary.sdf -# Ignition Gazebo 1.x.x and 2.x.x support using environment variables to -# configure distributed simulation. This capability is deprecated in -# version 2.x.x, and removed in verion 3.x.x of Inition Gazebo. Please use the -# --network-role and --network-secondaries command line options instead. - -# export IGN_GAZEBO_NETWORK_ROLE="SECONDARY" -# ign-gazebo -v 4 --distributed -f $DIR/secondary.sdf diff --git a/examples/scripts/distributed_levels/standalone.sdf b/examples/scripts/distributed_levels/standalone.sdf index b481243aba5..8b996ee96ca 100644 --- a/examples/scripts/distributed_levels/standalone.sdf +++ b/examples/scripts/distributed_levels/standalone.sdf @@ -23,75 +23,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/default/control - /world/default/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/default/stats - - - - - 0.8 0.8 0.8 1.0 diff --git a/examples/scripts/log_video_recorder/README.md b/examples/scripts/log_video_recorder/README.md index aa679d2f7da..3dc25c22d6f 100644 --- a/examples/scripts/log_video_recorder/README.md +++ b/examples/scripts/log_video_recorder/README.md @@ -34,14 +34,8 @@ timestamped directory where the `record_one_run.bash` is in. ## Changing camera follow behavior -The camera follow behavior can be configured by setting the `` -parameters in the GzScene3d GUI plugin in `log_video_recorder.sdf`, i.e. - - - 0.01 - true - -1.0 0 2.5 - +> This feature hasn't been ported to Fortress yet, see +> https://github.com/ignitionrobotics/ign-gui/issues/298 ## Troubleshooting diff --git a/examples/scripts/log_video_recorder/log_video_recorder.sdf b/examples/scripts/log_video_recorder/log_video_recorder.sdf index 9734ebfc857..e333935ddf5 100644 --- a/examples/scripts/log_video_recorder/log_video_recorder.sdf +++ b/examples/scripts/log_video_recorder/log_video_recorder.sdf @@ -34,7 +34,7 @@ - + 3D View false @@ -45,6 +45,24 @@ 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + 0.01 true @@ -70,6 +88,7 @@ true /world/default/control /world/default/stats + true @@ -91,6 +110,28 @@ true /world/default/stats + + + + false + 0 + 0 + 50 + 50 + floating + false + #777777 + + + + true + true + 4000000 + + + + false + diff --git a/examples/standalone/custom_server/CMakeLists.txt b/examples/standalone/custom_server/CMakeLists.txt index f0fc2311943..bc4a2c0cf1d 100644 --- a/examples/standalone/custom_server/CMakeLists.txt +++ b/examples/standalone/custom_server/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - find_package(ignition-gazebo5 REQUIRED) - set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) + find_package(ignition-gazebo6 REQUIRED) + set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) add_executable(custom_server custom_server.cc) target_link_libraries(custom_server diff --git a/examples/standalone/each_performance/CMakeLists.txt b/examples/standalone/each_performance/CMakeLists.txt index a8458e65522..98bc2b1b184 100644 --- a/examples/standalone/each_performance/CMakeLists.txt +++ b/examples/standalone/each_performance/CMakeLists.txt @@ -1,8 +1,8 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -find_package(ignition-gazebo5 QUIET REQUIRED) +find_package(ignition-gazebo6 QUIET REQUIRED) add_executable(each each.cc) target_link_libraries(each - ignition-gazebo5::core) + ignition-gazebo6::core) diff --git a/examples/standalone/entity_creation/CMakeLists.txt b/examples/standalone/entity_creation/CMakeLists.txt index e82d5965c88..ceb933a2005 100644 --- a/examples/standalone/entity_creation/CMakeLists.txt +++ b/examples/standalone/entity_creation/CMakeLists.txt @@ -1,7 +1,7 @@ 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-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) add_executable(entity_creation entity_creation.cc) target_link_libraries(entity_creation diff --git a/examples/standalone/external_ecm/CMakeLists.txt b/examples/standalone/external_ecm/CMakeLists.txt index 3f4c747cf75..f03b7c51d54 100644 --- a/examples/standalone/external_ecm/CMakeLists.txt +++ b/examples/standalone/external_ecm/CMakeLists.txt @@ -1,7 +1,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) -find_package(ignition-gazebo5 REQUIRED) +find_package(ignition-gazebo6 REQUIRED) add_executable(external_ecm external_ecm.cc) target_link_libraries(external_ecm - ignition-gazebo5::core) + ignition-gazebo6::core) diff --git a/examples/standalone/gtest_setup/CMakeLists.txt b/examples/standalone/gtest_setup/CMakeLists.txt index eb161340ebc..7fe6ac29027 100644 --- a/examples/standalone/gtest_setup/CMakeLists.txt +++ b/examples/standalone/gtest_setup/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.11.0 FATAL_ERROR) project(GTestSetup) # Find Gazebo -set(IGN_GAZEBO_VER 5) +set(IGN_GAZEBO_VER 6) find_package(ignition-gazebo${IGN_GAZEBO_VER} REQUIRED) # Fetch and configure GTest diff --git a/examples/standalone/joy_to_twist/CMakeLists.txt b/examples/standalone/joy_to_twist/CMakeLists.txt index dbda6c5867b..3f63c7255ce 100644 --- a/examples/standalone/joy_to_twist/CMakeLists.txt +++ b/examples/standalone/joy_to_twist/CMakeLists.txt @@ -1,10 +1,10 @@ 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-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) -find_package(sdformat11 REQUIRED) -set(SDF_VER ${sdformat11_VERSION_MAJOR}) +find_package(sdformat12 REQUIRED) +set(SDF_VER ${sdformat12_VERSION_MAJOR}) add_executable(joy_to_twist joy_to_twist.cc) target_link_libraries(joy_to_twist diff --git a/examples/standalone/joystick/CMakeLists.txt b/examples/standalone/joystick/CMakeLists.txt index 57d0dfc49c7..cea3c614afe 100644 --- a/examples/standalone/joystick/CMakeLists.txt +++ b/examples/standalone/joystick/CMakeLists.txt @@ -2,11 +2,11 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) # joystick currently works only on linux if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) - set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) + find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) + set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - find_package(sdformat11 REQUIRED) - set(SDF_VER ${sdformat11_VERSION_MAJOR}) + find_package(sdformat12 REQUIRED) + set(SDF_VER ${sdformat12_VERSION_MAJOR}) add_executable(joystick joystick.cc) target_link_libraries(joystick diff --git a/examples/standalone/keyboard/CMakeLists.txt b/examples/standalone/keyboard/CMakeLists.txt index b58d56794e3..2f7128cfbf9 100644 --- a/examples/standalone/keyboard/CMakeLists.txt +++ b/examples/standalone/keyboard/CMakeLists.txt @@ -1,14 +1,14 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) - set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) + find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) + set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) - find_package(sdformat11 REQUIRED) - set(SDF_VER ${sdformat11_VERSION_MAJOR}) + find_package(sdformat12 REQUIRED) + set(SDF_VER ${sdformat12_VERSION_MAJOR}) - find_package(ignition-msgs7 REQUIRED) - set(IGN_MSGS_VER ${ignition-msgs7_VERSION_MAJOR}) + find_package(ignition-msgs8 REQUIRED) + set(IGN_MSGS_VER ${ignition-msgs8_VERSION_MAJOR}) find_package(ignition-common4 REQUIRED) set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) diff --git a/examples/standalone/light_control/CMakeLists.txt b/examples/standalone/light_control/CMakeLists.txt index cad2d0d7ac9..ce5f63be0a1 100644 --- a/examples/standalone/light_control/CMakeLists.txt +++ b/examples/standalone/light_control/CMakeLists.txt @@ -1,10 +1,10 @@ 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-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) -find_package(ignition-gazebo5 REQUIRED) -set(IGN_GAZEBO_VER ${ignition-gazebo5_VERSION_MAJOR}) +find_package(ignition-gazebo6 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) add_executable(light_control light_control.cc) target_link_libraries(light_control diff --git a/examples/standalone/light_control/light_control.cc b/examples/standalone/light_control/light_control.cc index 4f499787c2b..8b5cf4118a2 100644 --- a/examples/standalone/light_control/light_control.cc +++ b/examples/standalone/light_control/light_control.cc @@ -137,7 +137,7 @@ int main(int argc, char **argv) m = std::sqrt(r*r + b*b + g*g); } r /= m; - b /= m; + g /= m; b /= m; //! [random numbers] diff --git a/examples/standalone/marker/CMakeLists.txt b/examples/standalone/marker/CMakeLists.txt deleted file mode 100644 index ee4be1fdd63..00000000000 --- a/examples/standalone/marker/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) - -if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") - find_package(ignition-transport10 QUIET REQUIRED OPTIONAL_COMPONENTS log) - set(IGN_TRANSPORT_VER ${ignition-transport10_VERSION_MAJOR}) - - find_package(ignition-common4 REQUIRED) - set(IGN_COMMON_VER ${ignition-common4_VERSION_MAJOR}) - - find_package(ignition-msgs7 REQUIRED) - set(IGN_MSGS_VER ${ignition-msgs7_VERSION_MAJOR}) - - add_executable(marker marker.cc) - target_link_libraries(marker - ignition-transport${IGN_TRANSPORT_VER}::core - ignition-msgs${IGN_MSGS_VER} - ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} - ) -endif() diff --git a/examples/standalone/marker/README.md b/examples/standalone/marker/README.md deleted file mode 100644 index 10da135f301..00000000000 --- a/examples/standalone/marker/README.md +++ /dev/null @@ -1,22 +0,0 @@ -# Ignition Visualization Marker Example - -This example demonstrates how to create, modify, and delete visualization -markers in Ignition Gazebo. - -## Build Instructions - -From this directory: - - mkdir build - cd build - cmake .. - make - -## Execute Instructions - -Launch ign gazebo unpaused then from the build directory above: - - ./marker - -The terminal will output messages indicating visualization changes that -will occur in Ignition Gazebo's render window. diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc deleted file mode 100644 index ec57227e837..00000000000 --- a/examples/standalone/marker/marker.cc +++ /dev/null @@ -1,315 +0,0 @@ -/* - * Copyright (C) 2019 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include -#include -#include - -#include - -///////////////////////////////////////////////// -int main(int _argc, char **_argv) -{ - ignition::transport::Node node; - - // Create the marker message - ignition::msgs::Marker markerMsg; - ignition::msgs::Material matMsg; - markerMsg.set_ns("default"); - markerMsg.set_id(0); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - markerMsg.set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Blue - markerMsg.mutable_material()->mutable_ambient()->set_r(0); - markerMsg.mutable_material()->mutable_ambient()->set_g(0); - markerMsg.mutable_material()->mutable_ambient()->set_b(1); - markerMsg.mutable_material()->mutable_ambient()->set_a(1); - markerMsg.mutable_material()->mutable_diffuse()->set_r(0); - markerMsg.mutable_material()->mutable_diffuse()->set_g(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(1); - markerMsg.mutable_material()->mutable_diffuse()->set_a(1); - markerMsg.mutable_lifetime()->set_sec(2); - markerMsg.mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - - // The rest of this function adds different shapes and/or modifies shapes. - // Read the terminal statements to figure out what each node.Request - // call accomplishes. - std::cout << "Spawning a blue sphere with lifetime 2s\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(2, 2, 0, 0, 0, 0)); - node.Request("/marker", markerMsg); - std::cout << "Sleeping for 2 seconds\n"; - std::this_thread::sleep_for(std::chrono::seconds(2)); - - std::cout << "Spawning a black sphere at the origin with no lifetime\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(1); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d::Zero); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - markerMsg.mutable_lifetime()->set_sec(0); - node.Request("/marker", markerMsg); - - std::cout << "Moving the black sphere to x=0, y=1, z=1\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 1, 1, 0, 0, 0)); - node.Request("/marker", markerMsg); - - std::cout << "Shrinking the black sphere\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.2, 0.2, 0.2)); - node.Request("/marker", markerMsg); - - std::cout << "Changing the black sphere to red\n"; - markerMsg.mutable_material()->mutable_ambient()->set_r(1); - markerMsg.mutable_material()->mutable_ambient()->set_g(0); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_r(1); - markerMsg.mutable_material()->mutable_diffuse()->set_g(0); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - std::this_thread::sleep_for(std::chrono::seconds(4)); - node.Request("/marker", markerMsg); - - std::cout << "Adding a green ellipsoid\n"; - markerMsg.mutable_material()->mutable_ambient()->set_r(0); - markerMsg.mutable_material()->mutable_ambient()->set_g(1); - markerMsg.mutable_material()->mutable_ambient()->set_b(0); - markerMsg.mutable_material()->mutable_diffuse()->set_r(0); - markerMsg.mutable_material()->mutable_diffuse()->set_g(1); - markerMsg.mutable_material()->mutable_diffuse()->set_b(0); - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(2); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.5, 1.0, 1.5)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(2, 0, .5, 0, 0, 0)); - node.Request("/marker", markerMsg); - - std::cout << "Changing the green ellipsoid to a cylinder\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_type(ignition::msgs::Marker::CYLINDER); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.5, 0.5, 1.5)); - node.Request("/marker", markerMsg); - - std::cout << "Connecting the sphere and cylinder with a line\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(3); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 0, 0, 0, 0, 0)); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 0, 0.5)); - node.Request("/marker", markerMsg); - - std::cout << "Adding a square around the origin\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(4); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_STRIP); - ignition::msgs::Set(markerMsg.mutable_point(0), - ignition::math::Vector3d(0.5, 0.5, 0.05)); - ignition::msgs::Set(markerMsg.mutable_point(1), - ignition::math::Vector3d(0.5, -0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(-0.5, -0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(-0.5, 0.5, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0.5, 0.5, 0.05)); - node.Request("/marker", markerMsg); - - std::cout << "Adding 100 points inside the square\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(5); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::POINTS); - markerMsg.clear_point(); - for (int i = 0; i < 100; ++i) - { - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d( - ignition::math::Rand::DblUniform(-0.49, 0.49), - ignition::math::Rand::DblUniform(-0.49, 0.49), - 0.05)); - } - node.Request("/marker", markerMsg); - - std::cout << "Adding a semi-circular triangle fan\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(6); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_FAN); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, 1.5, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - double radius = 2; - for (double t = 0; t <= M_PI; t+= 0.01) - { - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); - } - node.Request("/marker", markerMsg); - - std::cout << "Adding two triangles using a triangle list\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(7); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_LIST); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(0, -1.5, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(2, 2, 0.05)); - - node.Request("/marker", markerMsg); - - std::cout << "Adding a rectangular triangle strip\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_id(8); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::TRIANGLE_STRIP); - markerMsg.clear_point(); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(-2, -2, 0, 0, 0, 0)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 0, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 1, 0.05)); - - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 1, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(0, 2, 0.05)); - ignition::msgs::Set(markerMsg.add_point(), - ignition::math::Vector3d(1, 2, 0.05)); - - node.Request("/marker", markerMsg); - std::cout << "Adding multiple markers via /marker_array\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - - ignition::msgs::Marker_V markerMsgs; - ignition::msgs::Boolean res; - bool result; - unsigned int timeout = 5000; - - // Create first blue sphere marker - auto markerMsg1 = markerMsgs.add_marker(); - markerMsg1->set_ns("default"); - markerMsg1->set_id(0); - markerMsg1->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg1->set_type(ignition::msgs::Marker::SPHERE); - markerMsg1->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Blue - markerMsg1->mutable_material()->mutable_ambient()->set_r(0); - markerMsg1->mutable_material()->mutable_ambient()->set_g(0); - markerMsg1->mutable_material()->mutable_ambient()->set_b(1); - markerMsg1->mutable_material()->mutable_ambient()->set_a(1); - markerMsg1->mutable_material()->mutable_diffuse()->set_r(0); - markerMsg1->mutable_material()->mutable_diffuse()->set_g(0); - markerMsg1->mutable_material()->mutable_diffuse()->set_b(1); - markerMsg1->mutable_material()->mutable_diffuse()->set_a(1); - ignition::msgs::Set(markerMsg1->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg1->mutable_pose(), - ignition::math::Pose3d(3, 3, 0, 0, 0, 0)); - - // Create second red box marker - auto markerMsg2 = markerMsgs.add_marker(); - markerMsg2->set_ns("default"); - markerMsg2->set_id(0); - markerMsg2->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg2->set_type(ignition::msgs::Marker::BOX); - markerMsg2->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Red - markerMsg2->mutable_material()->mutable_ambient()->set_r(1); - markerMsg2->mutable_material()->mutable_ambient()->set_g(0); - markerMsg2->mutable_material()->mutable_ambient()->set_b(0); - markerMsg2->mutable_material()->mutable_ambient()->set_a(1); - markerMsg2->mutable_material()->mutable_diffuse()->set_r(1); - markerMsg2->mutable_material()->mutable_diffuse()->set_g(0); - markerMsg2->mutable_material()->mutable_diffuse()->set_b(0); - markerMsg2->mutable_material()->mutable_diffuse()->set_a(1); - markerMsg2->mutable_lifetime()->set_sec(2); - markerMsg2->mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg2->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg2->mutable_pose(), - ignition::math::Pose3d(3, 3, 2, 0, 0, 0)); - - // Create green capsule marker - auto markerMsg3 = markerMsgs.add_marker(); - markerMsg3->set_ns("default"); - markerMsg3->set_id(0); - markerMsg3->set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg3->set_type(ignition::msgs::Marker::CAPSULE); - markerMsg3->set_visibility(ignition::msgs::Marker::GUI); - - // Set color to Green - markerMsg3->mutable_material()->mutable_ambient()->set_r(0); - markerMsg3->mutable_material()->mutable_ambient()->set_g(1); - markerMsg3->mutable_material()->mutable_ambient()->set_b(0); - markerMsg3->mutable_material()->mutable_ambient()->set_a(1); - markerMsg3->mutable_material()->mutable_diffuse()->set_r(0); - markerMsg3->mutable_material()->mutable_diffuse()->set_g(1); - markerMsg3->mutable_material()->mutable_diffuse()->set_b(0); - markerMsg3->mutable_material()->mutable_diffuse()->set_a(1); - markerMsg3->mutable_lifetime()->set_sec(2); - markerMsg3->mutable_lifetime()->set_nsec(0); - ignition::msgs::Set(markerMsg3->mutable_scale(), - ignition::math::Vector3d(1.0, 1.0, 1.0)); - ignition::msgs::Set(markerMsg3->mutable_pose(), - ignition::math::Pose3d(3, 3, 4, 0, 0, 0)); - - // Publish the three created markers above simultaneously - node.Request("/marker_array", markerMsgs, timeout, res, result); - - std::cout << "Deleting all the markers\n"; - std::this_thread::sleep_for(std::chrono::seconds(4)); - markerMsg.set_action(ignition::msgs::Marker::DELETE_ALL); - node.Request("/marker", markerMsg); -} diff --git a/examples/standalone/multi_lrauv_race/CMakeLists.txt b/examples/standalone/multi_lrauv_race/CMakeLists.txt new file mode 100644 index 00000000000..5de19f32127 --- /dev/null +++ b/examples/standalone/multi_lrauv_race/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +find_package(ignition-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) + +find_package(ignition-gazebo6 REQUIRED) +set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR}) + +add_executable(multi_lrauv_race multi_lrauv_race.cc) +target_link_libraries(multi_lrauv_race + ignition-transport${IGN_TRANSPORT_VER}::core + ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}) diff --git a/examples/standalone/multi_lrauv_race/README.md b/examples/standalone/multi_lrauv_race/README.md new file mode 100644 index 00000000000..d9a2e2ee2b9 --- /dev/null +++ b/examples/standalone/multi_lrauv_race/README.md @@ -0,0 +1,25 @@ +# Multi-LRAUV Swimming Race Example + +This example shows the usage of the Thruster plugin and rudder joint control on +multiple autonomous underwater vehicles (AUV) with buoyancy, lift drag, and +hydrodynamics plugins. The multiple vehicles are differentiated by namespaces. + +## Build Instructions + +From this directory, run the following to compile: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +From the `build` directory, run Ignition and the example controller: + + ign gazebo -r ../../../worlds/multi_lrauv_race.sdf + ./multi_lrauv_race + +The example controller will output pseudorandom propeller and rudder commands +to move the vehicles forward. The low speed is by design to model the actual +vehicle velocity. diff --git a/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc new file mode 100644 index 00000000000..a180748fd1b --- /dev/null +++ b/examples/standalone/multi_lrauv_race/multi_lrauv_race.cc @@ -0,0 +1,109 @@ +/* + * 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. + * + */ + +/* + * In each iteration, for each vehicle, generate a random fin angle and thrust + * within reasonable limits, and send the command to the vehicle. + * + * Usage: + * $ multi_lrauv_race + */ + +#include +#include + +#include +#include + +// Fin joint limits from tethys model.sdf +double random_angle_within_limits(double min=-0.261799, double max=0.261799) +{ + return min + static_cast(rand()) / + (static_cast(RAND_MAX / (max - min))); +} + +// Nominal speed is thruster 300 rpm ~ 31.4 radians per second ~ 6.14 Newtons +double random_thrust_within_limits(double min=-6.14, double max=6.14) +{ + return min + static_cast(rand()) / + (static_cast(RAND_MAX / (max - min))); +} + +int main(int argc, char** argv) +{ + // Initialize random seed + srand(time(NULL)); + + std::vector ns; + ns.push_back("tethys"); + ns.push_back("triton"); + ns.push_back("daphne"); + + ignition::transport::Node node; + + std::vector rudderTopics; + rudderTopics.resize(ns.size(), ""); + std::vector rudderPubs; + rudderPubs.resize(ns.size()); + + std::vector propellerTopics; + propellerTopics.resize(ns.size(), ""); + std::vector propellerPubs; + propellerPubs.resize(ns.size()); + + // Set up topic names and publishers + for (int i = 0; i < ns.size(); i++) + { + rudderTopics[i] = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns[i] + "/joint/vertical_fins_joint/0/cmd_pos"); + rudderPubs[i] = node.Advertise(rudderTopics[i]); + + propellerTopics[i] = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns[i] + "/joint/propeller_joint/cmd_pos"); + propellerPubs[i] = node.Advertise( + propellerTopics[i]); + } + + std::vector rudderCmds; + rudderCmds.resize(ns.size(), 0.0); + std::vector propellerCmds; + propellerCmds.resize(ns.size(), 0.0); + + float artificial_speedup = 1; + + while (true) + { + for (int i = 0; i < ns.size(); i++) + { + rudderCmds[i] = random_angle_within_limits(-0.01, 0.01); + ignition::msgs::Double rudderMsg; + rudderMsg.set_data(rudderCmds[i]); + rudderPubs[i].Publish(rudderMsg); + + propellerCmds[i] = random_thrust_within_limits( + -6.14 * artificial_speedup, 0); + ignition::msgs::Double propellerMsg; + propellerMsg.set_data(propellerCmds[i]); + propellerPubs[i].Publish(propellerMsg); + + std::cout << "Commanding " << ns[i] << " rudder angle " << rudderCmds[i] + << " rad, thrust " << propellerCmds[i] << " Newtons" << std::endl; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + } +} diff --git a/examples/standalone/scene_requester/CMakeLists.txt b/examples/standalone/scene_requester/CMakeLists.txt index 4264cf811b8..ad388ffd853 100644 --- a/examples/standalone/scene_requester/CMakeLists.txt +++ b/examples/standalone/scene_requester/CMakeLists.txt @@ -1,7 +1,7 @@ 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-transport11 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(IGN_TRANSPORT_VER ${ignition-transport11_VERSION_MAJOR}) add_executable(scene_requester scene_requester.cc) target_link_libraries(scene_requester diff --git a/examples/worlds/3k_shapes.sdf b/examples/worlds/3k_shapes.sdf index e0f014f1a17..171a9cbe923 100644 --- a/examples/worlds/3k_shapes.sdf +++ b/examples/worlds/3k_shapes.sdf @@ -26,89 +26,6 @@ 0.8 0.8 0.8 - - - - - - 3D View - false - docked - - - ogre2 - scene - 1.0 1.0 1.0 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/shapes/control - /world/shapes/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/shapes/stats - - - - - - - false - docked - - - - - - - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/actor.sdf b/examples/worlds/actor.sdf index 346c511b26e..fc6af99daa8 100644 --- a/examples/worlds/actor.sdf +++ b/examples/worlds/actor.sdf @@ -22,89 +22,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/actors/control - /world/actors/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/actors/stats - - - - - - - docked - - - - - - - false - docked - - - - - true 0 0 10 0 0 0 @@ -248,5 +165,59 @@ + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + 1.0 + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + true + + + + diff --git a/examples/worlds/actor_crowd.sdf b/examples/worlds/actor_crowd.sdf index 545e475a1dc..b53d19e91a4 100644 --- a/examples/worlds/actor_crowd.sdf +++ b/examples/worlds/actor_crowd.sdf @@ -17,82 +17,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/actors/control - /world/actors/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/actors/stats - - - - - - - false - docked - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/actors_population.sdf.erb b/examples/worlds/actors_population.sdf.erb index 98bc2a2ad08..c885588bdef 100644 --- a/examples/worlds/actors_population.sdf.erb +++ b/examples/worlds/actors_population.sdf.erb @@ -21,82 +21,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/actors/control - /world/actors/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/actors/stats - - - - - - - false - docked - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/auv_controls.sdf b/examples/worlds/auv_controls.sdf index e82596c1520..224ed03ef53 100644 --- a/examples/worlds/auv_controls.sdf +++ b/examples/worlds/auv_controls.sdf @@ -1,4 +1,30 @@ + @@ -23,11 +49,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - - @@ -143,4 +164,4 @@ - \ No newline at end of file + diff --git a/examples/worlds/buoyancy.sdf b/examples/worlds/buoyancy.sdf index 66823419ed8..7ba29ec521e 100644 --- a/examples/worlds/buoyancy.sdf +++ b/examples/worlds/buoyancy.sdf @@ -44,74 +44,6 @@ 1000 - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/buoyancy/control - /world/buoyancy/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/buoyancy/stats - - - - true 0 0 10 0 0 0 @@ -641,4 +573,3 @@ - diff --git a/examples/worlds/buoyancy_engine.sdf b/examples/worlds/buoyancy_engine.sdf new file mode 100644 index 00000000000..145e955cab7 --- /dev/null +++ b/examples/worlds/buoyancy_engine.sdf @@ -0,0 +1,142 @@ + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 0 0 0 + + + 1000 + + 133.3333 + 133.3333 + 133.3333 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + body + buoyant_box + 0.0 + 0.002 + 0.002 + 0.003 + 0.0003 + + + + + diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf index b79e76b81bf..8e8c0907286 100644 --- a/examples/worlds/camera_sensor.sdf +++ b/examples/worlds/camera_sensor.sdf @@ -35,20 +35,56 @@ - + 3D View false docked - ogre + ogre2 scene - 1.0 1.0 1.0 + 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -69,6 +105,7 @@ true true true + true @@ -100,6 +137,20 @@ docked + + + + + docked + + + + + + + docked + + diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf new file mode 100644 index 00000000000..4769da875b0 --- /dev/null +++ b/examples/worlds/conveyor.sdf @@ -0,0 +1,349 @@ + + + + + + 0.004 + 1.0 + + + + + + + + + + 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 + + + + + 1 + + 0 0 0 0 0 0 + + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + 1 + 0 + + + + base_link + + + + + + + 87 + + + data: 10.0 + + + + + + + 88 + + + data: -1.0 + + + + + + + 83 + + + data: 0.0 + + + + + + 0 0 1 0 0 0 + + + 1.06 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 0 0 0 0 + + + 0.1 0.1 0.1 + + + + 1 1 1 1 + + + + + + 0.1 0.1 0.1 + + + 0 0 0 0 0 0 + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + Transform control + + + + + false + 230 + 50 + floating + false + #666666 + + + + + + + + + + + false + 200 + 50 + floating + false + #666666 + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + Visualize Contacts + + + + + false + 230 + 50 + floating + false + #ffffff + + + + + diff --git a/examples/worlds/depth_camera_sensor.sdf b/examples/worlds/depth_camera_sensor.sdf index ce693fe1b0a..80ce0abf932 100644 --- a/examples/worlds/depth_camera_sensor.sdf +++ b/examples/worlds/depth_camera_sensor.sdf @@ -25,7 +25,7 @@ - + 3D View false @@ -39,50 +39,41 @@ -6 0 6 0 0.5 0 - - + + - World control - false - false - 72 - 121 - 1 - - floating - - - - + floating + 5 + 5 + false - - true - true - true - - - - + - World stats - false - false - 110 - 290 - 1 - - floating - - - - + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false - - true - true - true - true diff --git a/examples/worlds/follow_actor.sdf b/examples/worlds/follow_actor.sdf index 468e3e68950..7570ae127d9 100644 --- a/examples/worlds/follow_actor.sdf +++ b/examples/worlds/follow_actor.sdf @@ -23,97 +23,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/actors/control - /world/actors/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/actors/stats - - - - - - - 0 - 0 - 263 - 50 - floating - false - #03a9f4 - - - - - - - false - docked - - - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/fuel_textured_mesh.sdf b/examples/worlds/fuel_textured_mesh.sdf index 7c2a39f95ad..2f42cf76081 100644 --- a/examples/worlds/fuel_textured_mesh.sdf +++ b/examples/worlds/fuel_textured_mesh.sdf @@ -37,7 +37,7 @@ - + 3D View false @@ -46,11 +46,48 @@ ogre2 scene - 1.0 1.0 1.0 + 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + @@ -71,6 +108,7 @@ true true true + true @@ -95,7 +133,6 @@ true true true - @@ -103,7 +140,6 @@ docked - diff --git a/examples/worlds/graded_buoyancy.sdf b/examples/worlds/graded_buoyancy.sdf new file mode 100644 index 00000000000..7b5b8936c71 --- /dev/null +++ b/examples/worlds/graded_buoyancy.sdf @@ -0,0 +1,287 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + 1000 + + 0 + 1 + + + + + lighter_than_water::ball::body + + + lighter_than_water::box + + + balloon_lighter_than_air + box_neutral_buoyancy + box_negative_buoyancy + + + + 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 + + + + + 100 100 + 0 0 1 + + + + 0 0 1 0.5 + 0 0 1 0.5 + 0 0 1 0.5 + + + + + + + 0 0 0 0 0 0 + + + + 0 0 0 0 0 0 + + 25 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + 3 5 0 0.3 0.2 0.1 + + + 200 + + 33.33 + 0 + 0 + 33.33 + 0 + 33.33 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + + 0 -5 -5 0 0 0 + + 0 0 0 0 0 0 + + 0.1 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + 0 5 -3 0 0 0 + + + 1000 + 0 0 0.1 0 0 0 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + 0 -8 0 0 0 0 + + + 1050 + 0 0 0.1 0 0 0 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + 4 -6 0 0 0 0 + + + 1050 + 0 0 0.1 0 0 0 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + diff --git a/examples/worlds/grid.sdf b/examples/worlds/grid.sdf index b7755445730..affe9141786 100644 --- a/examples/worlds/grid.sdf +++ b/examples/worlds/grid.sdf @@ -28,20 +28,38 @@ - + 3D View false docked - ogre + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -62,6 +80,7 @@ true true true + true @@ -86,7 +105,6 @@ true true true - @@ -95,11 +113,6 @@ Grid Config - - - - - diff --git a/examples/worlds/heightmap.sdf b/examples/worlds/heightmap.sdf index 6c2de6cb05c..c73203683fa 100644 --- a/examples/worlds/heightmap.sdf +++ b/examples/worlds/heightmap.sdf @@ -13,128 +13,6 @@ - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -80 40 60 0.0 0.4 -0.45 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - - Transform control - false - 0 - 0 - 250 - 50 - floating - false - #666666 - - - - - - - false - 250 - 0 - 150 - 50 - floating - false - #666666 - - - - - - - false - 400 - 0 - 150 - 50 - floating - false - #666666 - - - - - - - false - docked - - - - - - - false - docked - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/import_mesh.sdf b/examples/worlds/import_mesh.sdf index 1309625e2d9..685bace5d09 100644 --- a/examples/worlds/import_mesh.sdf +++ b/examples/worlds/import_mesh.sdf @@ -29,72 +29,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre2 - scene - 1.0 1.0 1.0 - 0.8 0.8 0.8 - -2 0 2 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/fuel/control - /world/fuel/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/fuel/stats - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/joint_controller.sdf b/examples/worlds/joint_controller.sdf index 86e54350a73..b3838229b78 100644 --- a/examples/worlds/joint_controller.sdf +++ b/examples/worlds/joint_controller.sdf @@ -25,44 +25,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 1 0 0.1 0 0.05 3.14 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - true - true - true - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/joint_position_controller.sdf b/examples/worlds/joint_position_controller.sdf index b2bd1045097..d1c09e43d46 100644 --- a/examples/worlds/joint_position_controller.sdf +++ b/examples/worlds/joint_position_controller.sdf @@ -19,46 +19,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 1 0 0.1 0 0.05 3.14 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/joint_trajectory_controller.sdf b/examples/worlds/joint_trajectory_controller.sdf index 556eb8ad526..6d5155129e3 100644 --- a/examples/worlds/joint_trajectory_controller.sdf +++ b/examples/worlds/joint_trajectory_controller.sdf @@ -154,64 +154,6 @@ 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 - - - diff --git a/examples/worlds/lightmap.sdf b/examples/worlds/lightmap.sdf index 8686531441f..51cd2ab8829 100644 --- a/examples/worlds/lightmap.sdf +++ b/examples/worlds/lightmap.sdf @@ -18,71 +18,6 @@ There are no dynamic lights or shadows in the scene. false - - - - - - 3D View - false - docked - - - ogre2 - scene - 1.0 1.0 1.0 - 0.8 0.8 0.8 - -5.5 -2 0.5 0 0.0 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - true Indoor Lightmap diff --git a/examples/worlds/minimal_scene.sdf b/examples/worlds/minimal_scene.sdf new file mode 100644 index 00000000000..90f85eac6d2 --- /dev/null +++ b/examples/worlds/minimal_scene.sdf @@ -0,0 +1,513 @@ + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + 0.25 + 25000 + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + false + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + + true + true + 4000000 + + + + false + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + false + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 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 + + + + 0.1458 + 0 + 0 + 0.1458 + 0 + 0.125 + + 1.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 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0 -3.0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 1 1 0 1 + 1 1 0 1 + 1 1 0 1 + + + + + + + 0 3.0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 1 0 1 1 + 1 0 1 1 + 1 0 1 1 + + + + + + diff --git a/examples/worlds/multi_lrauv_race.sdf b/examples/worlds/multi_lrauv_race.sdf new file mode 100644 index 00000000000..717bf861802 --- /dev/null +++ b/examples/worlds/multi_lrauv_race.sdf @@ -0,0 +1,365 @@ + + + + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + + + + + 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 + + + + + -5 0 0 0 0 0 + https://fuel.ignitionrobotics.org/1.0/mabel/models/Turquoise turbidity generator + + + + 0 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + 5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + triton + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + triton + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + -5 0 1 0 0 1.57 + https://fuel.ignitionrobotics.org/1.0/accurrent/models/MBARI Tethys LRAUV + daphne + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + daphne + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + + 0 0 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + start_line + + + 0 -25 -1 0 0 3.1415926 + https://fuel.ignitionrobotics.org/1.0/mabel/models/ABCSign_5m + finish_line + + + + diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 57e5bed01fc..effbeeafff7 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -12,6 +12,10 @@ You can use the velocity controller and command linear velocity and yaw angular ign topic -t "/X3/gazebo/command/twist" -m ignition.msgs.Twist -p " " + Listen to odometry: + + ign topic -e -t "/model/x3/odometry" + Send commands to the hexacopter to go straight up: @@ -20,6 +24,11 @@ You can use the velocity controller and command linear velocity and yaw angular To hover ign topic -t "/X4/gazebo/command/twist" -m ignition.msgs.Twist -p " " + + Listen to odometry: + + ign topic -e -t "/model/X4/odometry" + --> @@ -210,6 +219,11 @@ You can use the velocity controller and command linear velocity and yaw angular + + 3 + 0 3 1 0 0 0 @@ -400,6 +414,11 @@ You can use the velocity controller and command linear velocity and yaw angular + + 3 + diff --git a/examples/worlds/optical_tactile_sensor_plugin.sdf b/examples/worlds/optical_tactile_sensor_plugin.sdf index 08a900120fd..06262adb008 100644 --- a/examples/worlds/optical_tactile_sensor_plugin.sdf +++ b/examples/worlds/optical_tactile_sensor_plugin.sdf @@ -40,7 +40,7 @@ - + 3D View false @@ -51,10 +51,45 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - 0.35 0.23 0.94 0 0.05 -2.53 - + -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -75,10 +110,7 @@ true true true - /world/optical_tactile_plugin/control - /world/optical_tactile_plugin/stats - /world/depth_camera_sensor/control - /world/depth_camera_sensor/stats + true @@ -103,39 +135,37 @@ true true true - /world/optical_tactile_plugin/stats - /world/depth_camera_sensor/stats - + + - docked + docked_collapsed + + + + + + + docked_collapsed - - + - 0 - 0 - 263 - 50 + false + 5 + 5 floating false - #03a9f4 - - + - false - docked + docked - - - true diff --git a/examples/worlds/plane_propeller_demo.sdf b/examples/worlds/plane_propeller_demo.sdf index 6de0ef7e92b..d7f104cbf02 100644 --- a/examples/worlds/plane_propeller_demo.sdf +++ b/examples/worlds/plane_propeller_demo.sdf @@ -24,69 +24,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 20 0 3 0 0.0 3.14 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - true diff --git a/examples/worlds/plot_3d.sdf b/examples/worlds/plot_3d.sdf index afa33866c31..ed66fbf3a6c 100644 --- a/examples/worlds/plot_3d.sdf +++ b/examples/worlds/plot_3d.sdf @@ -14,7 +14,7 @@ - + 3D View false @@ -28,6 +28,42 @@ 5 -1.5 3 0 0.37 2.8 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -48,6 +84,8 @@ true true true + true + @@ -73,6 +111,16 @@ true + + + false + 5 + 5 + floating + false + + + diff --git a/examples/worlds/segmentation_camera.sdf b/examples/worlds/segmentation_camera.sdf new file mode 100644 index 00000000000..30d515cb4c0 --- /dev/null +++ b/examples/worlds/segmentation_camera.sdf @@ -0,0 +1,422 @@ + + + + + + + + + + + + ogre2 + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + docked_collapsed + + + + + + + docked_collapsed + + + + + + + + semantic/colored_map + + + + + panoptic/colored_map + + + + + semantic/labels_map + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + Car1 + -2 -2 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Hatchback blue + + + + + + + + Car2 + -3 -5 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pickup + + + + + + + + Car3 + -4 3 0 0 0 -1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/SUV + + + + + + + + + tree1 + -2 5 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + tree2 + -7 2 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + tree3 + -7 -4 0 0 0 0 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Pine Tree + + + + + + + + + home + -15 0 0 0 0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Collapsed House + + + + + + cone1 + 0 1 0 0 0 1.570796 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + cone2 + 0 4 0 0 0 1.570796 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + cone3 + 2 -2 0 0 0.0 1.57 + + https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Construction Cone + + + + + + + + + 6 0 2.0 0 0.0 3.14 + + 0 0 0 0 0 0 + + 0.5 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + panoptic + + instance + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + + + 1 + 30 + true + + + + + + + 6 0 2.0 0 0.0 3.14 + + 0 0 0 0 0 0 + + 0.5 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + semantic + + semantic + 1.57 + + 800 + 600 + + + 0.1 + 100 + + + + + 1 + 30 + true + + + + + + diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index eefd6a8ee8c..b07bd89925e 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -35,6 +35,10 @@ filename="ignition-gazebo-magnetometer-system" name="ignition::gazebo::systems::Magnetometer"> + + @@ -186,5 +190,71 @@ + + + true + + + + 0 0 2.0 0 0 0 + + + 0.100000 + 0.000000 + 0.000000 + 0.100000 + 0.000000 + 0.100000 + + 10.000000 + + + + + 0.100000 + + + + + 0 0 -0.75 0 0 0 + + + 0.0100000 + 1.5 + + + + + + + 0.100000 + + + + + + world + base_plate + + + base_plate + link_1 + 0 0 -1.5 0 0 0 + + + -1.57079 + 1.57079 + + + 0.000000 + 0.000000 + + 1.000000 0.000000 0.000000 + + + 10 + + + diff --git a/examples/worlds/sensors_demo.sdf b/examples/worlds/sensors_demo.sdf index 9e0d2414e33..31374a1a3e8 100644 --- a/examples/worlds/sensors_demo.sdf +++ b/examples/worlds/sensors_demo.sdf @@ -26,10 +26,11 @@ name="ignition::gazebo::systems::SceneBroadcaster"> + - + 3D View false @@ -43,6 +44,42 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -63,6 +100,7 @@ true true true + true @@ -89,7 +127,18 @@ true + + + + docked + + + + + + docked + diff --git a/examples/worlds/shapes_population.sdf.erb b/examples/worlds/shapes_population.sdf.erb index b65cc9c3ce2..91b52529c41 100644 --- a/examples/worlds/shapes_population.sdf.erb +++ b/examples/worlds/shapes_population.sdf.erb @@ -26,89 +26,6 @@ 0.8 0.8 0.8 - - - - - - 3D View - false - docked - - - ogre2 - scene - 1.0 1.0 1.0 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/shapes/control - /world/shapes/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/shapes/stats - - - - - - - false - docked - - - - - - - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/sky.sdf b/examples/worlds/sky.sdf index d555a0a0b45..be14b395c5a 100644 --- a/examples/worlds/sky.sdf +++ b/examples/worlds/sky.sdf @@ -10,9 +10,10 @@ Currently only supported using ogre2 rendering engine plugin. - + + - + 3D View false @@ -23,11 +24,46 @@ Currently only supported using ogre2 rendering engine plugin. scene 0.4 0.4 0.4 0.8 0.8 0.8 - - 6 0 1.0 0 0.0 3.14 + -6 0 6 0 0.5 0 - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + World control @@ -47,10 +83,11 @@ Currently only supported using ogre2 rendering engine plugin. true true true + true - + World stats @@ -71,20 +108,28 @@ Currently only supported using ogre2 rendering engine plugin. true true true - - RGB camera - 350 - 315 + docked - camera - + + + + docked + + + + + + docked + + + 0.001 @@ -302,4 +347,3 @@ Currently only supported using ogre2 rendering engine plugin. - diff --git a/examples/worlds/spherical_coordinates.sdf b/examples/worlds/spherical_coordinates.sdf new file mode 100644 index 00000000000..b8195168fb4 --- /dev/null +++ b/examples/worlds/spherical_coordinates.sdf @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + EARTH_WGS84 + ENU + -22.9 + -43.2 + 0 + 0 + + + + diff --git a/examples/worlds/thermal_camera.sdf b/examples/worlds/thermal_camera.sdf index e21215537a2..cb37b616190 100644 --- a/examples/worlds/thermal_camera.sdf +++ b/examples/worlds/thermal_camera.sdf @@ -31,7 +31,7 @@ - + 3D View false @@ -45,6 +45,42 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -65,8 +101,7 @@ true true true - /world/thermal_camera/control - /world/thermal_camera/stats + true @@ -91,7 +126,20 @@ true true true - /world/thermal_camera/stats + + + + + + docked + + + + + + + docked + diff --git a/examples/worlds/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf new file mode 100644 index 00000000000..cf9cd29acc1 --- /dev/null +++ b/examples/worlds/tracked_vehicle_simple.sdf @@ -0,0 +1,1822 @@ + + + + + 0.004 + 1.0 + + + + + + 1 1 1 1 + 0.8 0.8 0.8 1 + 1 + + + 1 + 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 + + + 1 + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 3 0 0.1 0 0 0 + + 0 0 0 0 0 0 + + -0.122 0 0.118 1.5708 0 0 + 13.14 + + 0.10019 + 0 + 0 + 0.345043 + 0 + 0.302044 + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + + -0.122 0 0.118 0 0 0 + + + 0.50017 0.24093 0.139 + + + + 0 + 1 + 0 + + + 0 0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + left_track + base_link + + + 0 -0.1985 0 0 0 0 + + 0 0 0.0141 0 0 0 + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0.01855 1.5708 0 1.5708 + + + 0.09728 0.18094 0.5 + + + + + 0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + + -0.25 0 0.01855 1.5708 0 0 + + + 0.09728 + 0.09047 + + + + 1 + 0 + + + right_track + base_link + + + 0.25 0.272 0.0195 0 -0.5 0 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 0.272 0.0195 3.14159 -0.5 3.14159 + + 0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + 0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + 0.165 0 0.0325 0 0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.165 0 -0.0325 0 -0.184162 0 + + + 0.33 0.04981 0.055 + + + + + 0.166 0 0.004 0 -0.02 0 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_left_flipper + left_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + 0.25 -0.272 0.0195 3.14159 0.5 3.14159 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + front_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + -0.25 -0.272 0.0195 0 0.5 0 + + -0.08 0 0 0 0 0 + 0.75 + + 0.0017491 + 2.8512e-07 + 0.0018277 + 0.012277 + -3.6288e-07 + 0.010941 + + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + + 20 + + + 0 0 0 1.5708 0 0 + + + 0.04981 + 0.089 + + + + + -0.33 0 0 1.5708 0 0 + + + 0.04981 + 0.029 + + + + + -0.165 0 0.0325 0 0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.165 0 -0.0325 0 -0.184162 -3.14159 + + + 0.33 0.04981 0.055 + + + + + -0.166 0 0.004 0 -0.02 -3.14159 + + + 0.2 0.04981 0.07 + + + + 1 + + 1 + 0 + + + rear_right_flipper + right_track + + 0 1 0 + + 0 + 0 + + + 0 + 0 + + + + + 1 + 1 + + 0 + 0.2 + + + + + + + left_track + front_left_flipper + rear_left_flipper + right_track + front_right_flipper + rear_right_flipper + 0.4 + 0.18094 + 0.5 + + + + left_track + -1.0 + 1.0 + + + + right_track + -1.0 + 1.0 + + + front_left_flipper + -1.0 + 1.0 + + + rear_left_flipper + -1.0 + 1.0 + + + front_right_flipper + -1.0 + 1.0 + + + rear_right_flipper + -1.0 + 1.0 + + + + + + 87 + + + linear: {x: 1.0}, angular: {z: 0.0} + + + + + + + 88 + + + linear: {x: -1.0}, angular: {z: 0.0} + + + + + + + 83 + + + linear: {x: 0.0}, angular: {z: 0.0} + + + + + + + 65 + + + linear: {x: 0.0}, angular: {z: 1.0} + + + + + + + 68 + + + linear: {x: 0.0}, angular: {z: -1.0} + + + + + + + 81 + + + linear: {x: 1.0}, angular: {z: 1.0} + + + + + + + 69 + + + linear: {x: 1.0}, angular: {z: -1.0} + + + + + + + 90 + + + linear: {x: -1.0}, angular: {z: 1.0} + + + + + + + 67 + + + linear: {x: -1.0}, angular: {z: -1.0} + + + + + 7 0 0 0 0 1.5708 + + + 0 1.6625 0.0375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.6625 0.0375 0 0 0 + + + 0 1.4875 0.1125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.4875 0.1125 0 0 0 + + + 0 1.3125 0.1875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.3125 0.1875 0 0 0 + + + 0 1.1375 0.2625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 1.1375 0.2625 0 0 0 + + + 0 0.9625 0.3375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.9625 0.3375 0 0 0 + + + 0 0.7875 0.4125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.7875 0.4125 0 0 0 + + + 0 0.6125 0.4875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.6125 0.4875 0 0 0 + + + 0 0.4375 0.5625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.4375 0.5625 0 0 0 + + + 0 0.2625 0.6375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.2625 0.6375 0 0 0 + + + 0 0.0875 0.7125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 0.0875 0.7125 0 0 0 + + + 0 -0.0875 0.7875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.0875 0.7875 0 0 0 + + + 0 -0.2625 0.8625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.2625 0.8625 0 0 0 + + + 0 -0.4375 0.9375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.4375 0.9375 0 0 0 + + + 0 -0.6125 1.0125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.6125 1.0125 0 0 0 + + + 0 -0.7875 1.0875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.7875 1.0875 0 0 0 + + + 0 -0.9625 1.1625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -0.9625 1.1625 0 0 0 + + + 0 -1.1375 1.2375 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.1375 1.2375 0 0 0 + + + 0 -1.3125 1.3125 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.3125 1.3125 0 0 0 + + + 0 -1.4875 1.3875 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.4875 1.3875 0 0 0 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 0.175 0.075 + + + + 1 1 1 1 + + + 0 + + + + + + 1 0.175 0.075 + + + 0 -1.6625 1.4625 0 0 0 + + + 1 + + + 1 + 0 0 -0.06 0 0 1.5708 + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + 1 + + + + + + 0 0 0 0 1.5708 0 + + + 0.15 + 0.8 + + + + + + + 1 + 2 2 0.028 1.7821 0 1.5708 + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + 1 + + + + + + 0 0 0 0 0 0 + + + 0.85 0.15 0.5 + + + + + + + pallet + 2 -2 0.1 0 0 0 + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Euro pallet + + + + + + 3D View + 0 + docked + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + World control + 0 + 0 + 72 + 121 + 1 + floating + + + + + + 1 + 1 + 1 + + + + World stats + 0 + 0 + 110 + 290 + 1 + floating + + + + + + 1 + 1 + 1 + 1 + + + + Transform control + + + + + 0 + 230 + 50 + floating + 0 + #666666 + + + + + + + + + 0 + 200 + 50 + floating + 0 + #666666 + + + + + + + + + false + 5 + 5 + floating + false + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + \ No newline at end of file diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index bb48095a064..303f78becab 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -39,72 +39,6 @@ start falling. name="ignition::gazebo::systems::Altimeter"> - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 3 -9 9 0 0.6 1.3 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/trisphere_cycle_wheel_slip.sdf b/examples/worlds/trisphere_cycle_wheel_slip.sdf index 77383613157..84ddecc6ff3 100644 --- a/examples/worlds/trisphere_cycle_wheel_slip.sdf +++ b/examples/worlds/trisphere_cycle_wheel_slip.sdf @@ -27,74 +27,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/wheel_slip/control - /world/wheel_slip/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/wheel_slip/stats - - - - true diff --git a/examples/worlds/tunnel.sdf b/examples/worlds/tunnel.sdf index 796d77fd7ba..583b3136412 100644 --- a/examples/worlds/tunnel.sdf +++ b/examples/worlds/tunnel.sdf @@ -24,7 +24,7 @@ - + 3D View false @@ -38,6 +38,77 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + Transform control + + + + + false + 230 + 50 + floating + false + #666666 + + + + false + + + + + + + + + + false + 200 + 50 + floating + false + #666666 + + @@ -58,6 +129,7 @@ true true true + true @@ -84,20 +156,6 @@ true - - - - docked_collapsed - - - - - - - docked_collapsed - - - Vehicle camera @@ -107,39 +165,19 @@ false - - + + - Transform control - - - - - false - 230 - 50 - floating - false - #666666 + docked_collapsed - - + + - - - - - false - 200 - 50 - floating - false - #666666 + docked_collapsed - @@ -155,6 +193,7 @@ + diff --git a/examples/worlds/velocity_control.sdf b/examples/worlds/velocity_control.sdf index 71c30486f92..1150268c3e3 100644 --- a/examples/worlds/velocity_control.sdf +++ b/examples/worlds/velocity_control.sdf @@ -29,75 +29,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -6 0 6 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - /world/velocity_control/control - /world/velocity_control/stats - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - /world/velocity_control/stats - - - - - true 0 0 10 0 0 0 diff --git a/examples/worlds/video_record_dbl_pendulum.sdf b/examples/worlds/video_record_dbl_pendulum.sdf index ddbe423ff36..5f0775c0467 100644 --- a/examples/worlds/video_record_dbl_pendulum.sdf +++ b/examples/worlds/video_record_dbl_pendulum.sdf @@ -31,7 +31,7 @@ - + 3D View false @@ -45,6 +45,121 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + false + 0 + 50 + 250 + 50 + floating + false + #777777 + + + + false + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + false + 5 + 5 + floating + false + + @@ -65,6 +180,7 @@ true true true + true @@ -89,13 +205,30 @@ true true true - - - + + + false + 300 + 50 + 50 + 50 + floating + false + #777777 + + + true + true + 4000000 + + + + false + diff --git a/examples/worlds/visibility.sdf b/examples/worlds/visibility.sdf index 01b2ca0048f..31bec366b30 100644 --- a/examples/worlds/visibility.sdf +++ b/examples/worlds/visibility.sdf @@ -42,7 +42,7 @@ - + 3D View false @@ -51,12 +51,47 @@ ogre2 scene - 1.0 1.0 1.0 + 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 - 0x05 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -77,8 +112,7 @@ true true true - /world/shapes/control - /world/shapes/stats + true @@ -103,8 +137,6 @@ true true true - /world/shapes/stats - @@ -126,6 +158,19 @@ false + + + + docked + + + + + + + docked + + diff --git a/examples/worlds/visualize_contacts.sdf b/examples/worlds/visualize_contacts.sdf index 5db0ca25d2f..f39994dcedd 100644 --- a/examples/worlds/visualize_contacts.sdf +++ b/examples/worlds/visualize_contacts.sdf @@ -1,7 +1,7 @@ - + 3D View false docked - ogre + ogre2 scene 0.4 0.4 0.4 0.8 0.8 0.8 -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -65,8 +102,7 @@ Contacts will be visualized as blue spheres and green cylinders. true true true - /world/visualize_contacts/control - /world/visualize_contacts/stats + true @@ -91,21 +127,16 @@ Contacts will be visualized as blue spheres and green cylinders. true true true - /world/visualize_contacts/stats - - - - - 0 - 0 - 263 - 50 - floating - false - #03a9f4 - + + + false + 5 + 5 + floating + false + diff --git a/examples/worlds/visualize_lidar.sdf b/examples/worlds/visualize_lidar.sdf index 311aa688c6e..d38fe690ce7 100644 --- a/examples/worlds/visualize_lidar.sdf +++ b/examples/worlds/visualize_lidar.sdf @@ -26,7 +26,7 @@ - + 3D View false @@ -40,6 +40,42 @@ -6 0 6 0 0.5 0 + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + @@ -60,8 +96,7 @@ true true true - /world/visualize_lidar_world/control - /world/visualize_lidar_world/stats + true @@ -86,11 +121,22 @@ true true true - /world/visualize_lidar_world/stats + + + + + + + docked + + + + + - docked + docked diff --git a/examples/worlds/wind.sdf b/examples/worlds/wind.sdf index d5d58b73b78..34cfceffa53 100644 --- a/examples/worlds/wind.sdf +++ b/examples/worlds/wind.sdf @@ -22,72 +22,6 @@ Example: name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre2 - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - -1 0 1 0 0.5 0 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - - - - - World stats - false - false - 110 - 290 - 1 - - floating - - - - - - - true - true - true - true - - - - - true 0 0 10 0 0 0 diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 089df6dd93d..a98c2b854b1 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -74,6 +75,41 @@ namespace ignition /// \return An id for the Entity, or kNullEntity on failure. public: Entity CreateEntity(); + /// \brief Clone an entity and its components. If the entity has any child + /// entities, they will also be cloned. + /// When cloning entities, the following rules apply: + /// 1. The name component of a cloned entity will consist of a unique + /// name, since all entities should have a unique name. + /// 2. Cloned entities that have a canonical link will have their + /// canonical link set to the cloned canonical link, not the original + /// canonical link. + /// 3. Child entities that are cloned will have their parent set to the + /// cloned parent entity. + /// 4. Cloned joints with parent/child links will have their parent and + /// child links set to the cloned parent/child links. + /// 5. Aside from the changes listed above, all other cloned components + /// remain unchanged. + /// Currently, cloning detachable joints is not supported. + /// \param[in] _entity The entity to clone. + /// \param[in] _parent The parent of the cloned entity. Set this to + /// kNullEntity if the cloned entity should not have a parent. + /// \param[in] _name The name that should be given to the cloned entity. + /// Set this to an empty string if the cloned entity name should be + /// auto-generated to something unique. + /// \param[in] _allowRename True if _name can be modified to be a unique + /// name if it isn't already a unique name. False if _name cannot be + /// modified to be a unique name. If _allowRename is set to False, and + /// _name is not unique, _entity will not be cloned. If _name is an + /// empty string, _allowRename is ignored since the cloned entity will + /// have an auto-generated unique name. + /// \return The cloned entity, which will have a unique name. kNullEntity + /// is returned if cloning failed. Failure could occur if _entity does not + /// exist, or if a unique name could not be generated for the entity to be + /// cloned. + /// \sa Clone + public: Entity Clone(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename); + /// \brief Get the number of entities on the server. /// \return Entity count. public: size_t EntityCount() const; @@ -160,7 +196,7 @@ namespace ignition /// \param[in] _entity The entity to check. /// \param[in] _key The component to check. /// \return True if the component key belongs to the entity. - public: bool EntityHasComponent(const Entity _entity, + public: bool IGN_DEPRECATED(6) EntityHasComponent(const Entity _entity, const ComponentKey &_key) const; /// \brief Check whether an entity has a specific component type. @@ -183,7 +219,7 @@ namespace ignition /// \param[in] _key A key that uniquely identifies a component. /// \return True if the entity and component existed and the component was /// removed. - public: bool RemoveComponent( + public: bool IGN_DEPRECATED(6) RemoveComponent( const Entity _entity, const ComponentKey &_key); /// \brief Remove a component from an entity based on a type id. @@ -211,9 +247,12 @@ namespace ignition /// \param[in] _entity The entity that will be associated with /// the component. /// \param[in] _data Data used to construct the component. - /// \return Key that uniquely identifies the component. + /// \return A pointer to the component that was created. nullptr is + /// returned if the component was not able to be created. If _entity + /// does not exist, nullptr will be returned. public: template - ComponentKey CreateComponent(const Entity _entity, + ComponentTypeT *CreateComponent( + const Entity _entity, const ComponentTypeT &_data); /// \brief Get a component assigned to an entity based on a @@ -237,14 +276,16 @@ namespace ignition /// \return The component associated with the key, or nullptr if the /// component could not be found. public: template - const ComponentTypeT *Component(const ComponentKey &_key) const; + const ComponentTypeT IGN_DEPRECATED(6) * Component( + const ComponentKey &_key) const; /// \brief Get a mutable component based on a key. /// \param[in] _key A key that uniquely identifies a component. /// \return The component associated with the key, or nullptr if the /// component could not be found. public: template - ComponentTypeT *Component(const ComponentKey &_key); + ComponentTypeT IGN_DEPRECATED(6) * 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 @@ -253,7 +294,7 @@ namespace ignition /// \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. + /// entity. If _entity does not exist, nullptr is returned. public: template ComponentTypeT *ComponentDefault(Entity _entity, const typename ComponentTypeT::Type &_default = @@ -292,20 +333,20 @@ namespace ignition Entity _entity) const; /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. + /// This function is now deprecated, and will always return nullptr. + /// \return nullptr. public: template - const ComponentTypeT *First() const; + const ComponentTypeT IGN_DEPRECATED(6) * First() const; /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. + /// This function is now deprecated, and will always return nullptr. + /// \return nullptr. public: template - ComponentTypeT *First(); + ComponentTypeT IGN_DEPRECATED(6) * First(); /// \brief Get an entity which matches the value of all the given /// components. For example, the following will return the entity which - /// has an name component equal to "name" and has a model component: + /// has a name component equal to "name" and has a model component: /// /// auto entity = EntityByComponents(components::Name("name"), /// components::Model()); @@ -357,6 +398,21 @@ namespace ignition private: template struct identity; // NOLINT + /// \brief Helper function for cloning an entity and its children (this + /// includes cloning components attached to these entities). This method + /// should never be called directly - it is called internally from the + /// public Clone method. + /// \param[in] _entity The entity to clone. + /// \param[in] _parent The parent of the cloned entity. + /// \param[in] _name The name that should be given to the cloned entity. + /// \param[in] _allowRename True if _name can be modified to be a unique + /// name if it isn't already a unique name. False if _name cannot be + /// modified to be a unique name. + /// \return The cloned entity. kNullEntity is returned if cloning failed. + /// \sa Clone + private: Entity CloneImpl(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename); + /// \brief A version of Each() that doesn't use a cache. The cached /// version, Each(), is preferred. /// Get all entities which contain given component types, as well @@ -554,7 +610,7 @@ namespace ignition /// and components. /// \details The header of the message will not be populated, it is the /// responsibility of the caller to timestamp it before use. - /// \param[in] _state serialized state + /// \param[out] _state The serialized state message to populate. /// \param[in] _entities Entities to be serialized. Leave empty to get /// all entities. /// \param[in] _types Type ID of components to be serialized. Leave empty @@ -643,24 +699,14 @@ namespace ignition /// \return True if the Entity has been marked to be removed. private: bool IsMarkedForRemoval(const Entity _entity) const; - /// \brief Delete an existing Entity. - /// \param[in] _entity The entity to remove. - /// \returns True if the Entity existed and was deleted. - private: bool RemoveEntity(const Entity _entity); - - /// \brief The first component instance of the specified type. - /// \return First component instance of the specified type, or nullptr - /// if the type does not exist. - private: components::BaseComponent *First( - const ComponentTypeId _componentTypeId); - /// \brief Implementation of CreateComponent. /// \param[in] _entity The entity that will be associated with /// the component. /// \param[in] _componentTypeId Id of the component type. /// \param[in] _data Data used to construct the component. - /// \return Key that uniquely identifies the component. - private: ComponentKey CreateComponentImplementation( + /// \return True if the component's data needs to be set externally; false + /// otherwise. + private: bool CreateComponentImplementation( const Entity _entity, const ComponentTypeId _componentTypeId, const components::BaseComponent *_data); @@ -683,79 +729,31 @@ namespace ignition const Entity _entity, const ComponentTypeId _type); - /// \brief Get a component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - private: const components::BaseComponent *ComponentImplementation( - const ComponentKey &_key) const; - - /// \brief Get a mutable component based on a key. - /// \param[in] _key A key that uniquely identifies a component. - /// \return The component associated with the key, or nullptr if the - /// component could not be found. - private: components::BaseComponent *ComponentImplementation( - const ComponentKey &_key); - - /// \brief End of the AddComponentToView recursion. This function is - /// called when Rest is empty. - /// \param[in, out] _view The FirstComponent will be added to the - /// _view. - /// \param[in] _entity The entity. - private: template::type = 0> - void AddComponentsToView(detail::View &_view, - const Entity _entity) const; - - /// \brief Recursively add components to a view. This function is - /// called when Rest is NOT empty. - /// \param[in, out] _view The FirstComponent will be added to the - /// _view. - /// \param[in] _entity The entity. - private: template::type = 0> - void AddComponentsToView(detail::View &_view, - const Entity _entity) const; - /// \brief Find a View that matches the set of ComponentTypeIds. If /// a match is not found, then a new view is created. /// \tparam ComponentTypeTs All the component types that define a view. - /// \return A reference to the view. + /// \return A pointer to the view. private: template - detail::View &FindView() const; + detail::View *FindView() const; /// \brief Find a view based on the provided component type ids. /// \param[in] _types The component type ids that serve as a key into /// a map of views. - /// \param[out] _iter Iterator to the found element in the view map. - /// Check the return value to see if this iterator is valid. - /// \return True if the view was found, false otherwise. - private: bool FindView(const std::set &_types, - std::map::iterator &_iter) const; // NOLINT + /// \return A pair containing a the view itself and a mutex that can be + /// used for locking the view while entities are being added to it. + /// If a view defined by _types does not exist, the pair will contain + /// nullptrs. + private: std::pair FindView( + const std::vector &_types) const; /// \brief Add a new view to the set of stored views. - /// \param[in] _types The set of component type ids that is the key + /// \param[in] _types The set of component type ids that act as the key /// for the view. /// \param[in] _view The view to add. - /// \return An iterator to the view. - private: std::map::iterator - AddView(const std::set &_types, - detail::View &&_view) const; - - /// \brief Update views that contain the provided entity. - /// \param[in] _entity The entity. - private: void UpdateViews(const Entity _entity); - - /// \brief Get a component ID based on an entity and the component's type. - /// \param[in] _entity The entity. - /// \param[in] _type Component type ID. - private: ComponentId EntityComponentIdFromType( - const Entity _entity, const ComponentTypeId _type) const; + /// \return A pointer to the view. + private: detail::BaseView *AddView( + const detail::ComponentTypeKey &_types, + std::unique_ptr _view) const; /// \brief Add an entity and its components to a serialized state message. /// \param[out] _msg The state message. @@ -783,6 +781,22 @@ namespace ignition const std::unordered_set &_types = {}, bool _full = false) const; + /// \brief Set whether views should be locked when entities are being + /// added to them. This can be used to prevent race conditions in + /// system PostUpdates, since these are run in parallel (entities are + /// added to views when the view is used, so if two systems try to access + /// the same view in PostUpdate, we run the risk of multiple threads + /// reading/writing from the same data). + /// \param[in] _lock Whether the views should lock while entities are + /// being added to them (true) or not (false). + private: void LockAddingEntitiesToViews(bool _lock); + + /// \brief Get whether views should be locked when entities are being + /// added to them. + /// \return True if views should be locked during entitiy addition, false + /// otherwise. + private: bool LockAddingEntitiesToViews() const; + // Make runners friends so that they can manage entity creation and // removal. This should be safe since runners are internal // to Gazebo. @@ -793,10 +807,6 @@ namespace ignition // states. Like the runners, the managers are internal. friend class NetworkManagerPrimary; friend class NetworkManagerSecondary; - - // Make View a friend so that it can access components. - // This should be safe since View is internal to Gazebo. - friend class detail::View; }; } } diff --git a/include/ignition/gazebo/Primitives.hh b/include/ignition/gazebo/Primitives.hh new file mode 100644 index 00000000000..eff9a97b837 --- /dev/null +++ b/include/ignition/gazebo/Primitives.hh @@ -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. + * + */ + +#ifndef IGNITION_GAZEBO_PRIMITIVES_HH_ +#define IGNITION_GAZEBO_PRIMITIVES_HH_ + +#include +#include + +#include + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + /// \brief Enumeration of available primitive shape types + enum class IGNITION_GAZEBO_VISIBLE PrimitiveShape + { + kBox, + kCapsule, + kCylinder, + kEllipsoid, + kSphere, + }; + + /// \brief Enumeration of available primitive light types + enum class IGNITION_GAZEBO_VISIBLE PrimitiveLight + { + kDirectional, + kPoint, + kSpot, + }; + + /// \brief Return an SDF string of one of the available primitive + /// shape types + /// \param[in] _type Type of shape to retrieve + /// \return String containing SDF description of primitive shape + /// Empty string if the _type is not supported. + std::string IGNITION_GAZEBO_VISIBLE + getPrimitiveShape(const PrimitiveShape &_type); + + /// \brief Return an SDF string of one of the available primitive + /// light types + /// \param[in] _type Type of light to retrieve + /// \return String containing SDF description of primitive light + /// Empty string if the _type is not supported. + std::string IGNITION_GAZEBO_VISIBLE + getPrimitiveLight(const PrimitiveLight &_type); + + /// \brief Return an SDF string of one of the available primitive shape or + /// light types. + /// \param[in] _typeName Type name of the of shape or light to retrieve. + /// Must be one of: box, sphere, cylinder, capsule, ellipsoid, directional, + /// point, or spot. + /// \return String containing SDF description of primitive shape or light. + /// Empty string if the _typeName is invalid. + std::string IGNITION_GAZEBO_VISIBLE + getPrimitive(const std::string &_typeName); + } + } // namespace gazebo +} // namespace ignition + + +#endif // IGNITION_GAZEBO_PRIMITIVES_HH_ + + diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 438c716c486..a975cc7bc4f 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -244,16 +244,6 @@ namespace ignition /// \param[in] _recordPath Path to place recorded states public: void SetLogRecordPath(const std::string &_recordPath); - /// \brief Get whether to ignore the path specified in SDF. - /// \return Whether to ignore the path specified in SDF - public: bool IGN_DEPRECATED(4) LogIgnoreSdfPath() const; - - /// \brief Set whether to ignore the path specified in SDF. Path in SDF - /// should be ignored if a record path is specified on the command line, - /// for example. - /// \param[in] _ignore Whether to ignore the path specified in SDF - public: void IGN_DEPRECATED(4) SetLogIgnoreSdfPath(bool _ignore); - /// \brief Add a topic to record. /// \param[in] _topic Topic name, which can include wildcards. public: void AddLogRecordTopic(const std::string &_topic); @@ -335,6 +325,14 @@ namespace ignition /// \return File containing render engine library. public: const std::string &RenderEngineGui() const; + /// \brief Set the headless mode + /// \param[in] _headless Set to true to enable headless mode. + public: void SetHeadlessRendering(const bool _headless); + + /// \brief Get the headless mode + /// \return True if headless mode is enable, false otherwise. + public: bool HeadlessRendering() const; + /// \brief Set the render engine server plugin library. /// \param[in] _renderEngineServer File containing render engine library. public: void SetRenderEngineServer( diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 84026a0c9e2..67dbe2ea3bd 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -22,6 +22,8 @@ #include #include +#include "ignition/gazebo/Entity.hh" + namespace ignition { namespace gazebo @@ -78,6 +80,8 @@ namespace ignition /// \brief A unique identifier for a component instance. The uniqueness /// of a ComponentId is scoped to the component's type. /// \sa ComponentKey. + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. using ComponentId = int; /// \brief A unique identifier for a component type. A component type @@ -87,7 +91,10 @@ namespace ignition /// \brief A key that uniquely identifies, at the global scope, a component /// instance - using ComponentKey = std::pair; + /// \note On version 6, the 2nd element was changed to the entity ID. + /// \deprecated Deprecated on version 6, removed on version 7. Use + /// ComponentTypeId + Entity instead. + using ComponentKey = std::pair; /// \brief typedef for query callbacks using EntityQueryCallback = std::function IGNITION_GAZEBO_VISIBLE sphericalCoordinates( + Entity _entity, const EntityComponentManager &_ecm); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"}; @@ -251,7 +260,6 @@ namespace ignition /// \brief Environment variable holding paths to custom rendering engine /// plugins. const std::string kRenderPluginPathEnv{"IGN_GAZEBO_RENDER_ENGINE_PATH"}; - } } } diff --git a/include/ignition/gazebo/World.hh b/include/ignition/gazebo/World.hh index 55d98ea54bd..9e6d8ccfb02 100644 --- a/include/ignition/gazebo/World.hh +++ b/include/ignition/gazebo/World.hh @@ -24,6 +24,7 @@ #include #include +#include #include "ignition/gazebo/config.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -116,11 +117,24 @@ namespace ignition /// \brief Get atmosphere information. /// \param[in] _ecm Entity-component manager. - /// \return Magnetic field vector or nullopt if the entity does not + /// \return Atmosphere or nullopt if the entity does not /// have a components::Atmosphere component. public: std::optional Atmosphere( const EntityComponentManager &_ecm) const; + /// \brief Get spherical coordinates for the world origin. + /// \param[in] _ecm Entity-component manager. + /// \return Spherical coordinates or nullopt if the entity does not + /// have a components::SphericalCoordinates component. + public: std::optional SphericalCoordinates( + const EntityComponentManager &_ecm) const; + + /// \brief Set spherical coordinates for the world origin. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _sphericalCoordinates New spherical coordinates. + public: void SetSphericalCoordinates(EntityComponentManager &_ecm, + const math::SphericalCoordinates &_sphericalCoordinates); + /// \brief Get the ID of a light entity which is an immediate child of /// this world. /// \param[in] _ecm Entity-component manager. diff --git a/include/ignition/gazebo/components/Collision.hh b/include/ignition/gazebo/components/Collision.hh index 9c57c90c4c7..f35f0e95d96 100644 --- a/include/ignition/gazebo/components/Collision.hh +++ b/include/ignition/gazebo/components/Collision.hh @@ -51,6 +51,15 @@ namespace components serializers::CollisionElementSerializer>; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CollisionElement", CollisionElement) + + /// \brief A component used to enable customization of contact surface for a + /// collision. The customization itself is done in callback of event + /// CollectContactSurfaceProperties from PhysicsEvents. + using EnableContactSurfaceCustomization = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.EnableContactSurfaceCustomization", + EnableContactSurfaceCustomization) } } } diff --git a/include/ignition/gazebo/components/Component.hh b/include/ignition/gazebo/components/Component.hh index bbee6d34c51..59eee24fb6c 100644 --- a/include/ignition/gazebo/components/Component.hh +++ b/include/ignition/gazebo/components/Component.hh @@ -279,6 +279,10 @@ namespace components /// Factory registration and is guaranteed to be the same across compilers /// and runs. public: virtual ComponentTypeId TypeId() const = 0; + + /// \brief Clone the component. + /// \return A pointer to the component. + public: virtual std::unique_ptr Clone() = 0; }; /// \brief A component type that wraps any data type. The intention is for @@ -342,6 +346,9 @@ namespace components /// \return True if different. public: bool operator!=(const Component &_component) const; + // Documentation inherited + public: std::unique_ptr Clone() override; + // Documentation inherited public: ComponentTypeId TypeId() const override; @@ -408,6 +415,9 @@ namespace components public: bool operator!=(const Component &_component) const; + // Documentation inherited + public: std::unique_ptr Clone() override; + // Documentation inherited public: ComponentTypeId TypeId() const override; @@ -490,6 +500,16 @@ namespace components Serializer::Deserialize(_in, this->Data()); } + ////////////////////////////////////////////////// + template + std::unique_ptr + Component::Clone() + { + Component clonedComp(this->Data()); + return std::make_unique>( + clonedComp); + } + ////////////////////////////////////////////////// template ComponentTypeId Component::TypeId() const @@ -513,6 +533,14 @@ namespace components return false; } + ////////////////////////////////////////////////// + template + std::unique_ptr + Component::Clone() + { + return std::make_unique>(); + } + ////////////////////////////////////////////////// template ComponentTypeId Component::TypeId() const diff --git a/include/ignition/gazebo/components/CustomSensor.hh b/include/ignition/gazebo/components/CustomSensor.hh new file mode 100644 index 00000000000..98712492416 --- /dev/null +++ b/include/ignition/gazebo/components/CustomSensor.hh @@ -0,0 +1,45 @@ +/* + * 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_CUSTOMSENSOR_HH_ +#define IGNITION_GAZEBO_COMPONENTS_CUSTOMSENSOR_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 type that contains a custom sensor's information. + /// A custom sensor is any sensor that's not officially supported through + /// the SDF spec. + using CustomSensor = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.CustomSensor", + CustomSensor) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/Factory.hh b/include/ignition/gazebo/components/Factory.hh index 0b7a17184de..5e280a906c1 100644 --- a/include/ignition/gazebo/components/Factory.hh +++ b/include/ignition/gazebo/components/Factory.hh @@ -49,6 +49,12 @@ namespace components /// \brief Create an instance of a Component. /// \return Pointer to a component. public: virtual std::unique_ptr Create() const = 0; + + /// \brief Create an instance of a Component, populated with specific data. + /// \param[in] _data The data to populate the component with. + /// \return Pointer to a component. + public: virtual std::unique_ptr Create( + const components::BaseComponent *_data) const = 0; }; /// \brief A class for an object responsible for creating components. @@ -57,17 +63,27 @@ namespace components class ComponentDescriptor : public ComponentDescriptorBase { - /// \brief Create an instance of a ComponentTypeT Component. - /// \return Pointer to a component. + /// \brief Documentation inherited public: std::unique_ptr Create() const override { return std::make_unique(); } + + /// \brief Documentation inherited + public: virtual std::unique_ptr Create( + const components::BaseComponent *_data) const override + { + ComponentTypeT comp(*static_cast(_data)); + return std::make_unique(comp); + } }; /// \brief A base class for an object responsible for creating storages. class StorageDescriptorBase { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptorBase() = default; + /// \brief Destructor public: virtual ~StorageDescriptorBase() = default; @@ -82,6 +98,9 @@ namespace components class StorageDescriptor : public StorageDescriptorBase { + /// \brief Constructor + public: IGN_DEPRECATED(6) StorageDescriptor() = default; + /// \brief Create an instance of a storage that holds ComponentTypeT /// components. /// \return Pointer to a component. @@ -100,12 +119,25 @@ namespace components /// \param[in] _type Type of component to register. /// \param[in] _compDesc Object to manage the creation of ComponentTypeT /// objects. - /// \param[in] _storageDesc Object to manage the creation of storages for - /// objects of type ComponentTypeT. + /// \param[in] _storageDesc Ignored. /// \tparam ComponentTypeT Type of component to register. + /// \deprecated See function that doesn't accept a storage public: template - void Register(const std::string &_type, ComponentDescriptorBase *_compDesc, - StorageDescriptorBase *_storageDesc) + void IGN_DEPRECATED(6) Register(const std::string &_type, + ComponentDescriptorBase *_compDesc, + StorageDescriptorBase * /*_storageDesc*/) + { + this->Register(_type, _compDesc); + } + + /// \brief Register a component so that the factory can create instances + /// of the component based on an ID. + /// \param[in] _type Type of component to register. + /// \param[in] _compDesc Object to manage the creation of ComponentTypeT + /// objects. + /// \tparam ComponentTypeT Type of component to register. + public: template + void Register(const std::string &_type, ComponentDescriptorBase *_compDesc) { // Every time a plugin which uses a component type is loaded, it attempts // to register it again, so we skip it. @@ -153,13 +185,12 @@ namespace components // Keep track of all types this->compsById[ComponentTypeT::typeId] = _compDesc; - this->storagesById[ComponentTypeT::typeId] = _storageDesc; namesById[ComponentTypeT::typeId] = ComponentTypeT::typeName; runtimeNamesById[ComponentTypeT::typeId] = runtimeName; } /// \brief Unregister a component so that the factory can't create instances - /// of the component or its storage anymore. + /// of the component anymore. /// \tparam ComponentTypeT Type of component to unregister. public: template void Unregister() @@ -170,7 +201,7 @@ namespace components } /// \brief Unregister a component so that the factory can't create instances - /// of the component or its storage anymore. + /// of the component anymore. /// \details This function will not reset the `typeId` static variable /// within the component type itself. Prefer using the templated /// `Unregister` function when possible. @@ -192,15 +223,6 @@ namespace components } } - { - auto it = this->storagesById.find(_typeId); - if (it != this->storagesById.end()) - { - delete it->second; - this->storagesById.erase(it); - } - } - { auto it = namesById.find(_typeId); if (it != namesById.end()) @@ -245,19 +267,45 @@ namespace components return comp; } + /// \brief Create a new instance of a component, initialized with particular + /// data. + /// \param[in] _type Component id to create. + /// \param[in] _data The data to populate the component instance with. + /// \return Pointer to a component. Null if the component + /// type could not be handled. + public: std::unique_ptr New( + const ComponentTypeId &_type, const components::BaseComponent *_data) + { + std::unique_ptr comp; + + if (nullptr == _data) + { + ignerr << "Requested to create a new component instance with null " + << "data." << std::endl; + } + else if (_type != _data->TypeId()) + { + ignerr << "The typeID of _type [" << _type << "] does not match the " + << "typeID of _data [" << _data->TypeId() << "]." << std::endl; + } + else + { + auto it = this->compsById.find(_type); + if (it != this->compsById.end() && nullptr != it->second) + comp = it->second->Create(_data); + } + + return comp; + } + /// \brief Create a new instance of a component storage. /// \param[in] _typeId Type of component which the storage will hold. - /// \return Pointer to a storage. Null if the component type could not be - /// handled. - public: std::unique_ptr NewStorage( - const ComponentTypeId &_typeId) + /// \return Always returns nullptr. + /// \deprecated Storages aren't necessary anymore. + public: std::unique_ptr IGN_DEPRECATED(6) NewStorage( + const ComponentTypeId & /*_typeId*/) { - std::unique_ptr storage; - auto it = this->storagesById.find(_typeId); - if (it != this->storagesById.end() && nullptr != it->second) - storage = it->second->Create(); - - return storage; + return nullptr; } /// \brief Get all the registered component types by ID. @@ -305,10 +353,6 @@ namespace components /// we just keep a pointer, which will dangle until the program is shutdown. private: std::map compsById; - /// \brief A list of registered storages where the key is its component's - /// type id. - private: std::map storagesById; - /// \brief A list of IDs and their equivalent names. public: std::map namesById; @@ -335,9 +379,8 @@ namespace components return; \ using namespace ignition;\ using Desc = gazebo::components::ComponentDescriptor<_classname>; \ - using StorageDesc = gazebo::components::StorageDescriptor<_classname>; \ gazebo::components::Factory::Instance()->Register<_classname>(\ - _compType, new Desc(), new StorageDesc());\ + _compType, new Desc());\ } \ }; \ static IgnGazeboComponents##_classname\ diff --git a/include/ignition/gazebo/components/ForceTorque.hh b/include/ignition/gazebo/components/ForceTorque.hh new file mode 100644 index 00000000000..12da2780774 --- /dev/null +++ b/include/ignition/gazebo/components/ForceTorque.hh @@ -0,0 +1,47 @@ +/* + * 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_FORCETORQUE_HH_ +#define IGNITION_GAZEBO_COMPONENTS_FORCETORQUE_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 A component type that contains an FT sensor, + /// sdf::ForceTorque, information. + using ForceTorque = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ForceTorque", + ForceTorque) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/JointEffortLimitsCmd.hh b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh new file mode 100644 index 00000000000..b8b9217974b --- /dev/null +++ b/include/ignition/gazebo/components/JointEffortLimitsCmd.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_COMPONENTS_JOINTEFFORTLIMITSCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_JOINTEFFORTLIMITSCMD_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 Command for setting effort limits of a joint. Data are a vector +/// with a Vector2 for each DOF. The X() component of the Vector2 specifies +/// the minimum effort limit, the Y() component stands for maximum limit. +/// Set to +-infinity to disable the limits. +/// \note It is expected that the physics plugin reads this component and +/// sets the limit to the dynamics engine. After setting it, the data of this +/// component will be cleared (i.e. the vector will have length zero). +using JointEffortLimitsCmd = Component< + std::vector, + class JointEffortLimitsCmdTag, + serializers::VectorSerializer +>; + +IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointEffortLimitsCmd", JointEffortLimitsCmd) +} + +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/JointPositionLimitsCmd.hh b/include/ignition/gazebo/components/JointPositionLimitsCmd.hh new file mode 100644 index 00000000000..775937fbff5 --- /dev/null +++ b/include/ignition/gazebo/components/JointPositionLimitsCmd.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_COMPONENTS_JOINTPOSITIONLIMITSCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_JOINTPOSITIONLIMITSCMD_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 Command for setting position limits of a joint. Data are a vector +/// with a Vector2 for each DOF. The X() component of the Vector2 specifies +/// the minimum positional limit, the Y() component stands for maximum limit. +/// Set to +-infinity to disable the limits. +/// \note It is expected that the physics plugin reads this component and +/// sets the limit to the dynamics engine. After setting it, the data of this +/// component will be cleared (i.e. the vector will have length zero). +using JointPositionLimitsCmd = Component< + std::vector, + class JointPositionLimitsCmdTag, + serializers::VectorSerializer +>; + +IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointPositionLimitsCmd", JointPositionLimitsCmd) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/JointTransmittedWrench.hh b/include/ignition/gazebo/components/JointTransmittedWrench.hh new file mode 100644 index 00000000000..10cef6a294f --- /dev/null +++ b/include/ignition/gazebo/components/JointTransmittedWrench.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_JOINTTRANSMITTEDWRENCH_HH_ +#define IGNITION_GAZEBO_COMPONENTS_JOINTTRANSMITTEDWRENCH_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 Joint Transmitted wrench in SI units (Nm for torque, N for force). +/// The wrench is expressed in the Joint frame and the force component is +/// applied at the joint origin. +/// \note The term Wrench is used here to mean a pair of 3D vectors representing +/// torque and force quantities expessed in a given frame and where the force is +/// applied at the origin of the frame. This is different from the Wrench used +/// in screw theory. +/// \note The value of force_offset in msgs::Wrench is ignored for this +/// component. The force is assumed to be applied at the origin of the joint +/// frame. +using JointTransmittedWrench = + Component; +IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointTransmittedWrench", + JointTransmittedWrench) +} // namespace components +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh new file mode 100644 index 00000000000..e85905095f4 --- /dev/null +++ b/include/ignition/gazebo/components/JointVelocityLimitsCmd.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_COMPONENTS_JOINTVELOCITYLIMITSCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_JOINTVELOCITYLIMITSCMD_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 Command for setting velocity limits of a joint. Data are a vector +/// with a Vector2 for each DOF. The X() component of the Vector2 specifies +/// the minimum velocity limit, the Y() component stands for maximum limit. +/// Set to +-infinity to disable the limits. +/// \note It is expected that the physics plugin reads this component and +/// sets the limit to the dynamics engine. After setting it, the data of this +/// component will be cleared (i.e. the vector will have length zero). +using JointVelocityLimitsCmd = Component< + std::vector, + class JointVelocityLimitsCmdTag, + serializers::VectorSerializer +>; + +IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.JointVelocityLimitsCmd", JointVelocityLimitsCmd) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/Model.hh b/include/ignition/gazebo/components/Model.hh index 7fdc7014ce4..5ba0420d942 100644 --- a/include/ignition/gazebo/components/Model.hh +++ b/include/ignition/gazebo/components/Model.hh @@ -17,7 +17,10 @@ #ifndef IGNITION_GAZEBO_COMPONENTS_MODEL_HH_ #define IGNITION_GAZEBO_COMPONENTS_MODEL_HH_ +#include + #include +#include #include #include @@ -29,6 +32,54 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + class SdfModelSerializer + { + /// \brief Serialization for `sdf::Model`. + /// \param[in] _out Output stream. + /// \param[in] _time Model to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const sdf::Model &_model) + { + sdf::ElementPtr modelElem = _model.Element(); + if (!modelElem) + { + ignerr << "Unable to serialize sdf::Model" << std::endl; + return _out; + } + + _out << "" + << "" + << modelElem->ToString("") + << ""; + return _out; + } + + /// \brief Deserialization for `sdf::Model`. + /// \param[in] _in Input stream. + /// \param[out] _model Model to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + sdf::Model &_model) + { + sdf::Root root; + std::string sdf(std::istreambuf_iterator(_in), {}); + + sdf::Errors errors = root.LoadSdfString(sdf); + if (!root.Model()) + { + ignerr << "Unable to unserialize sdf::Model" << std::endl; + return _in; + } + + _model = *root.Model(); + return _in; + } + }; +} + namespace components { /// \brief A component that identifies an entity as being a model. @@ -36,7 +87,9 @@ namespace components IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.Model", Model) /// \brief A component that holds the model's SDF DOM - using ModelSdf = Component; + using ModelSdf = Component; IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.ModelSdf", ModelSdf) } } diff --git a/include/ignition/gazebo/components/RenderEngineServerHeadless.hh b/include/ignition/gazebo/components/RenderEngineServerHeadless.hh new file mode 100644 index 00000000000..9746e4b0aa2 --- /dev/null +++ b/include/ignition/gazebo/components/RenderEngineServerHeadless.hh @@ -0,0 +1,45 @@ +/* + * 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_RENDERENGINESERVERHEADLESS_HH_ +#define IGNITION_GAZEBO_COMPONENTS_RENDERENGINESERVERHEADLESS_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 Holds the headless mode. + using RenderEngineServerHeadless = Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.RenderEngineServerHeadless", + RenderEngineServerHeadless) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/SegmentationCamera.hh b/include/ignition/gazebo/components/SegmentationCamera.hh new file mode 100644 index 00000000000..c476fc8ba82 --- /dev/null +++ b/include/ignition/gazebo/components/SegmentationCamera.hh @@ -0,0 +1,45 @@ +/* + * 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_SEGMENTATIONCAMERA_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SEGMENTATIONCAMERA_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 type that contains a Segmentation camera sensor, + /// sdf::Camera, information. + using SegmentationCamera = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SegmentationCamera", + SegmentationCamera) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/SemanticLabel.hh b/include/ignition/gazebo/components/SemanticLabel.hh new file mode 100644 index 00000000000..3139b753105 --- /dev/null +++ b/include/ignition/gazebo/components/SemanticLabel.hh @@ -0,0 +1,43 @@ +/* + * 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_LABEL_HH_ +#define IGNITION_GAZEBO_COMPONENTS_LABEL_HH_ + +#include "ignition/gazebo/Export.hh" +#include "ignition/gazebo/components/Component.hh" +#include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component that holds the label of an entity. One example use + /// case of the Label component is with Segmentation & Bounding box + /// sensors to generate dataset annotations. + using SemanticLabel = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.SemanticLabel", + SemanticLabel) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 7fd3ee7765e..2916058dc2b 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -25,6 +25,7 @@ #include #include +#include // This header holds serialization operators which are shared among several // components @@ -35,6 +36,31 @@ namespace gazebo { // Inline bracket to help doxygen filtering. inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace traits +{ + /// \brief Type trait that determines if an ignition::gazebo::convert from In + /// to Out is defined. + /// Usage: + /// \code + /// constexpr bool hasGazeboConvert = + /// HasGazeboConvert::value + /// \endcode + template + class HasGazeboConvert + { + private: template + static auto Test(int _test) + -> decltype(std::declval() = + ignition::gazebo::convert(std::declval()), + std::true_type()); + + private: template + static auto Test(...) -> std::false_type; + + public: static constexpr bool value = // NOLINT + decltype(Test(true))::value; + }; +} /// \brief A Serializer class is used to serialize and deserialize a component. /// It is passed in as the third template parameter to components::Component. /// Eg. @@ -78,7 +104,16 @@ namespace serializers public: static std::ostream &Serialize(std::ostream &_out, const DataType &_data) { - auto msg = ignition::gazebo::convert(_data); + MsgType msg; + if constexpr (traits::HasGazeboConvert::value) + { + msg = ignition::gazebo::convert(_data); + } + else + { + msg = ignition::msgs::Convert(_data); + } + msg.SerializeToOstream(&_out); return _out; } @@ -93,7 +128,14 @@ namespace serializers MsgType msg; msg.ParseFromIstream(&_in); - _data = ignition::gazebo::convert(msg); + if constexpr (traits::HasGazeboConvert::value) + { + _data = ignition::gazebo::convert(msg); + } + else + { + _data = ignition::msgs::Convert(msg); + } return _in; } }; @@ -183,6 +225,40 @@ namespace serializers return _in; } }; + + template + class VectorSerializer + { + /// \brief Serialization for `std::vector` with serializable T. + /// \param[in] _out Output stream. + /// \param[in] _data The data to stream. + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const std::vector &_data) + { + _out << _data.size(); + for (const auto& datum : _data) + _out << " " << datum; + return _out; + } + + /// \brief Deserialization for `std::vector` with serializable T. + /// \param[in] _in Input stream. + /// \param[out] _data The data to populate. + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, + std::vector &_data) + { + size_t size; + _in >> size; + _data.resize(size); + for (size_t i = 0; i < size; ++i) + { + _in >> _data[i]; + } + return _in; + } + }; } } } diff --git a/include/ignition/gazebo/components/SphericalCoordinates.hh b/include/ignition/gazebo/components/SphericalCoordinates.hh new file mode 100644 index 00000000000..93ebd43fa82 --- /dev/null +++ b/include/ignition/gazebo/components/SphericalCoordinates.hh @@ -0,0 +1,53 @@ +/* + * 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_SPHERICALCOORDINATES_HH_ +#define IGNITION_GAZEBO_COMPONENTS_SPHERICALCOORDINATES_HH_ + +#include +#include +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace serializers +{ + using SphericalCoordinatesSerializer = + serializers::ComponentToMsgSerializer; +} + +namespace components +{ + /// \brief This component holds the spherical coordinates of the world origin. + using SphericalCoordinates = + Component; + IGN_GAZEBO_REGISTER_COMPONENT( + "ign_gazebo_components.SphericalCoordinates", SphericalCoordinates) +} +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/VisualCmd.hh b/include/ignition/gazebo/components/VisualCmd.hh new file mode 100644 index 00000000000..794057aabb7 --- /dev/null +++ b/include/ignition/gazebo/components/VisualCmd.hh @@ -0,0 +1,48 @@ +/* + * 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_VISUALCMD_HH_ +#define IGNITION_GAZEBO_COMPONENTS_VISUALCMD_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 visual of an + /// entity in the world frame represented by msgs::Visual. + using VisualCmd = Component; + IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.VisualCmd", + VisualCmd) +} +} +} +} +#endif diff --git a/include/ignition/gazebo/detail/BaseView.hh b/include/ignition/gazebo/detail/BaseView.hh new file mode 100644 index 00000000000..eea67553169 --- /dev/null +++ b/include/ignition/gazebo/detail/BaseView.hh @@ -0,0 +1,219 @@ +/* + * 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_DETAIL_BASEVIEW_HH_ +#define IGNITION_GAZEBO_DETAIL_BASEVIEW_HH_ + +#include +#include +#include +#include + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/config.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace detail +{ +/// \brief A key into the map of views +using ComponentTypeKey = std::vector; + +/// \brief Hash functor for ComponentTypeKey. +/// The implementation was inspired by: +/// * https://stackoverflow.com/a/20511429 +/// * https://stackoverflow.com/a/4948967 +/// * https://stackoverflow.com/a/35991300 +/// * https://softwareengineering.stackexchange.com/a/402543 +struct ComponentTypeHasher +{ + std::size_t operator()(const std::vector &_vec) const + { + auto hash = _vec.size(); + for (const auto &i : _vec) + hash ^= i + 0x9e3779b9 + (hash << 6) + (hash >> 2); + return hash; + } +}; + +/// \brief A view is a cache to entities, and their components, that +/// match a set of component types. A cache is used because systems will +/// frequently, potentially every iteration, query the +/// EntityComponentManager for sets of entities that match a set of +/// component types. Rather than look up the entities every time, we can +/// use a cache to improve performance. +/// +/// Note that symbols for this class are visible because methods from this class +/// are used in templated Ignition::Gazebo::EntityComponentManager methods. +/// However, users should not use this class (or anything else in namespace +/// ignition::gazebo::detail) directly. +class IGNITION_GAZEBO_VISIBLE BaseView +{ + /// \brief Destructor + public: virtual ~BaseView() = default; + + /// \brief See if an entity is a part of the view + /// \param[in] _entity The entity + /// \return true if _entity is a part of the view, false otherwise + public: bool HasEntity(const Entity _entity) const; + + /// \brief See if an entity is marked as an entity to be added to the view + /// \param[in] _entity The entity + /// \return true if _entity is to be added to the view, false otherwise + public: bool IsEntityMarkedForAddition(const Entity _entity) const; + + /// \brief See if the view has cached the component data for an entity + /// \param[in] _entity The entity + /// \return true if _entity has component data cached in the view, false + /// otherwise + public: virtual bool HasCachedComponentData(const Entity _entity) const = 0; + + /// \brief Save an entity as one to be added to the view, if the entity isn't + /// already a part of the view. This entity's component data should be added + /// to the view the next time the view is being used. + /// \param[in] _entity The entity to be added + /// \param[in] _new Whether to add the entity to the list of new entities. + /// The new here is to indicate whether the entity is new to the entity + /// component manager. An existing entity can be added when creating a new + /// view or when rebuilding the view. + /// \return True if _entity isn't already a part of the view and was marked as + /// an entity to be added. False otherwise + /// \sa HasEntity, IsEntityMarkedForAddition + public: bool MarkEntityToAdd(const Entity _entity, bool _new = false); + + /// \brief See if the view requires a particular component type + /// \param[in] _typeId The component type + /// \return true if the view requires components of type _typeId, false + /// otherwise + public: bool RequiresComponent(const ComponentTypeId _typeId) const; + + /// \brief Update the internal data in the view because a component has been + /// added to an entity. It is assumed that the entity is already associated + /// with the view, and that the added component type is required by the view. + /// \param[in] _entity The entity + /// \param[in] _newEntity Whether to add the entity to the list of new + /// entities. The new here is to indicate whether the entity is new to the + /// entity component manager. + /// \param[in] _typeId The type of component that was added to _entity + /// \return true if the notification related to the component addition of type + /// _typeId occurred for _entity, false otherwise + /// \sa HasCachedComponentData, RequiresComponent + public: virtual bool NotifyComponentAddition(const Entity _entity, + bool _newEntity, const ComponentTypeId _typeId) = 0; + + /// \brief Update the internal data in the view because a component has been + /// removed to an entity. It is assumed that the entity is already associated + /// with the view, and that the removed component type is required by the view + /// \param[in] _entity The entity + /// \param[in] _typeId The type of component that was removed from _entity + /// \return true if the notification related to the component removal of type + /// _typeId occurred for _entity, false otherwise + /// \sa HasCachedComponentData, RequiresComponent + public: virtual bool NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) = 0; + + /// \brief Remove an entity from the view. + /// \param[in] _entity The entity to remove. + /// \return True if the entity was removed, false if the entity did not + /// exist in the view. + public: virtual bool RemoveEntity(const Entity _entity) = 0; + + /// \brief Add the entity to the list of entities to be removed + /// \param[in] _entity The entity to add. + /// \return True if the entity was added to the list, false if the entity + /// was not associated with the view. + public: bool MarkEntityToRemove(const Entity _entity); + + /// \brief Update the entities in the view to no longer appear as newly + /// created. This method should be called whenever a new simulation step is + /// about to take place. + public: void ResetNewEntityState(); + + /// \brief Get the set of component types that this view requires. + /// \return The set of component types. + public: const std::set &ComponentTypes() const; + + /// \brief Clear all data from the view and reset it to its original, empty + /// state. + public: virtual void Reset() = 0; + + /// \brief Get all of the entities in the view + /// \return The entities in the view + public: const std::set &Entities() const; + + /// \brief Get all of the entities in the view that are considered "newly + /// created". While an entity may be new to the view, it may not be a newly + /// created entity (perhaps this entity has existed for some time, and just + /// had a component added to it that now makes this entity a part of the + /// view). An entity's "newness" is determined by the entity component + /// manager. + /// \return The newly created entities that are a part of the view + public: const std::set &NewEntities() const; + + /// \brief Get all of the entities to be removed from the view + /// \return The entities to be removed from the view + public: const std::set &ToRemoveEntities() const; + + /// \brief Get all of the entities that should be added to the view. This is + /// useful for adding entities to the view before the view is used to ensure + /// that the view is up-to-date. Once all entities marked "to be added" are + /// added to the view, the ClearToAddEntities method should be called. + /// \return The entities to be added to the view, with the key of the map + /// indicating whether an entity is a newly created entity or not. + /// \sa ClearToAddEntities + public: const std::unordered_map &ToAddEntities() const; + + /// \brief Clear all of the entities that are marked as to be added to the + /// view. This should be called after all of the entities marked as to be + /// added to the view are actually added to the view. + /// \sa ToAddEntities + public: void ClearToAddEntities(); + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief All the entities that belong to this view. + protected: std::set entities; + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief List of newly created entities + protected: std::set newEntities; + + // TODO(adlarkin) make this a std::unordered_set for better performance. + // We need to make sure nothing else depends on the ordered preserved by + // std::set first + /// \brief List of entities about to be removed + protected: std::set toRemoveEntities; + + /// \brief List of entities to be added to the view. The value of the map + /// indicates whether the entity is new to the entity component manager or not + protected: std::unordered_map toAddEntities; + + /// \brief The component types in the view + protected: std::set componentTypes; +}; +} // namespace detail +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition +#endif diff --git a/include/ignition/gazebo/detail/ComponentStorageBase.hh b/include/ignition/gazebo/detail/ComponentStorageBase.hh index ca9e5e89603..cb4e0edcc21 100644 --- a/include/ignition/gazebo/detail/ComponentStorageBase.hh +++ b/include/ignition/gazebo/detail/ComponentStorageBase.hh @@ -17,12 +17,7 @@ #ifndef IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ #define IGNITION_GAZEBO_DETAIL_COMPONENTSTORAGEBASE_HH_ -#include -#include -#include -#include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Types.hh" namespace ignition { @@ -37,47 +32,10 @@ namespace ignition class IGNITION_GAZEBO_HIDDEN ComponentStorageBase { /// \brief Constructor - public: ComponentStorageBase() = default; + public: IGN_DEPRECATED(6) ComponentStorageBase() = default; /// \brief Destructor public: virtual ~ComponentStorageBase() = default; - - /// \brief Create a new component using the provided data. - /// \param[in] _data Data used to construct the component. - /// \return Id of the new component, and whether the components array - /// was expanded. kComponentIdInvalid is returned - /// if the component could not be created. - public: virtual std::pair Create( - const components::BaseComponent *_data) = 0; - - /// \brief Remove a component based on an id. - /// \param[in] _id Id of the component to remove. - /// \return True if the component was removed. - public: virtual bool Remove(const ComponentId _id) = 0; - - /// \brief Remove all components - public: virtual void RemoveAll() = 0; - - /// \brief Get a component based on an id. - /// \param[in] _id Id of the component to get. - /// \return A pointer to the component, or nullptr if the component - /// could not be found. - public: virtual const components::BaseComponent *Component( - const ComponentId _id) const = 0; - - /// \brief Get a mutable component based on an id. - /// \param[in] _id Id of the component to get. - /// \return A pointer to the component, or nullptr if the component - /// could not be found. - public: virtual components::BaseComponent *Component( - const ComponentId _id) = 0; - - /// \brief Get the first component. - /// \return First component or nullptr if there are no components. - public: virtual components::BaseComponent *First() = 0; - - /// \brief Mutex used to prevent data corruption. - protected: mutable std::mutex mutex; }; /// \brief Templated implementation of component storage. @@ -85,137 +43,10 @@ namespace ignition class IGNITION_GAZEBO_HIDDEN ComponentStorage : public ComponentStorageBase { /// \brief Constructor - public: explicit ComponentStorage() + public: explicit IGN_DEPRECATED(6) ComponentStorage() : ComponentStorageBase() { - // Reserve a chunk of memory for the components. The size here will - // effect how often Views are rebuilt when - // EntityComponentManager::CreateComponent() is called. - // - // Views would be rebuilt if the components vector capacity is - // exceeded after an EntityComponentManager::Each call has already - // been executed. - // - // See also this class's Create() function, which expands the value - // of components vector whenever the capacity is reached. - this->components.reserve(100); } - - // Documentation inherited. - public: bool Remove(const ComponentId _id) final - { - std::lock_guard lock(this->mutex); - - // Get an iterator to the component that should be removed. - auto iter = this->idMap.find(_id); - - // Make sure the component exists. - if (iter != this->idMap.end()) - { - // Handle the case where there are more components than the - // component to be removed - if (this->components.size() > 1) - { - // Swap the component to be removed with the component at the - // back of the vector. - std::swap(this->components[iter->second], - this->components.back()); - - // After the swap, we have to fix all the id mappings. - for (auto idIter =this->idMap.begin(); - idIter != this->idMap.end(); ++idIter) - { - if (static_cast(idIter->second) == - this->components.size()-1) - { - idIter->second = iter->second; - } - } - } - - // Remove the component. - this->components.pop_back(); - - // Remove the id mapping. - this->idMap.erase(iter); - return true; - } - return false; - } - - // Documentation inherited. - public: void RemoveAll() final - { - this->idCounter = 0; - this->idMap.clear(); - this->components.clear(); - } - - // Documentation inherited. - public: std::pair Create( - const components::BaseComponent *_data) final - { - ComponentId result; // = kComponentIdInvalid; - bool expanded = false; - if (this->components.size() == this->components.capacity()) - { - this->components.reserve(this->components.capacity() + 100); - expanded = true; - } - - std::lock_guard lock(this->mutex); - // cppcheck-suppress unmatchedSuppression - // cppcheck-suppress postfixOperator - result = this->idCounter++; - this->idMap[result] = this->components.size(); - // Copy the component - this->components.push_back(std::move( - ComponentTypeT(*static_cast(_data)))); - - return {result, expanded}; - } - - // Documentation inherited. - public: const components::BaseComponent *Component( - const ComponentId _id) const final - { - return static_cast( - const_cast *>( - this)->Component(_id)); - } - - public: components::BaseComponent *Component(const ComponentId _id) final - { - std::lock_guard lock(this->mutex); - - auto iter = this->idMap.find(_id); - - if (iter != this->idMap.end()) - { - return static_cast( - &this->components.at(iter->second)); - } - return nullptr; - } - - // Documentation inherited. - public: components::BaseComponent *First() final - { - std::lock_guard lock(this->mutex); - if (!this->components.empty()) - return static_cast(&this->components[0]); - return nullptr; - } - - /// \brief The id counter is used to get unique ids within this - /// storage class. - private: ComponentId idCounter = 0; - - /// \brief Map of ComponentId to Components (see the components vector). - private: std::map idMap; - - /// \brief Sequential storage of components. - public: std::vector components; }; } } diff --git a/include/ignition/gazebo/detail/EntityComponentManager.hh b/include/ignition/gazebo/detail/EntityComponentManager.hh index d08722ee803..b92beb81c86 100644 --- a/include/ignition/gazebo/detail/EntityComponentManager.hh +++ b/include/ignition/gazebo/detail/EntityComponentManager.hh @@ -19,8 +19,11 @@ #include #include +#include +#include #include #include +#include #include #include #include @@ -81,11 +84,24 @@ auto CompareData = [](const DataType &_a, const DataType &_b) -> bool ////////////////////////////////////////////////// template -ComponentKey EntityComponentManager::CreateComponent(const Entity _entity, +ComponentTypeT *EntityComponentManager::CreateComponent(const Entity _entity, const ComponentTypeT &_data) { - return this->CreateComponentImplementation(_entity, ComponentTypeT::typeId, - &_data); + auto updateData = this->CreateComponentImplementation(_entity, + ComponentTypeT::typeId, &_data); + auto comp = this->Component(_entity); + if (updateData) + { + if (!comp) + { + ignerr << "Internal error. Failure to create a component of type " + << ComponentTypeT::typeId << " for entity " << _entity + << ". This should never happen!\n"; + return comp; + } + *comp = _data; + } + return comp; } ////////////////////////////////////////////////// @@ -117,7 +133,7 @@ const ComponentTypeT *EntityComponentManager::Component( const ComponentKey &_key) const { return static_cast( - this->ComponentImplementation(_key)); + this->ComponentImplementation(_key.second, _key.first)); } ////////////////////////////////////////////////// @@ -125,7 +141,7 @@ template ComponentTypeT *EntityComponentManager::Component(const ComponentKey &_key) { return static_cast( - this->ComponentImplementation(_key)); + this->ComponentImplementation(_key.second, _key.first)); } ////////////////////////////////////////////////// @@ -174,16 +190,18 @@ bool EntityComponentManager::SetComponentData(const Entity _entity, template const ComponentTypeT *EntityComponentManager::First() const { - return static_cast( - this->First(ComponentTypeT::typeId)); + ignwarn << "EntityComponentManager::First is now deprecated and will always " + << "return nullptr.\n"; + return nullptr; } ////////////////////////////////////////////////// template ComponentTypeT *EntityComponentManager::First() { - return static_cast( - this->First(ComponentTypeT::typeId)); + ignwarn << "EntityComponentManager::First is now deprecated and will always " + << "return nullptr.\n"; + return nullptr; } ////////////////////////////////////////////////// @@ -196,7 +214,7 @@ Entity EntityComponentManager::EntityByComponents( // Iterate over entities Entity result{kNullEntity}; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { bool different{false}; @@ -234,7 +252,7 @@ std::vector EntityComponentManager::EntitiesByComponents( // Iterate over entities std::vector result; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { bool different{false}; @@ -274,7 +292,7 @@ std::vector EntityComponentManager::ChildrenByComponents(Entity _parent, // Iterate over entities std::vector result; - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { if (children.find(entity) == children.end()) { @@ -361,13 +379,13 @@ void EntityComponentManager::Each(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view, and invoke the callback // function. - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -381,13 +399,13 @@ void EntityComponentManager::Each(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view, and invoke the callback // function. - for (const Entity entity : view.entities) + for (const Entity entity : view->Entities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentData(entity))) { break; } @@ -409,14 +427,14 @@ void EntityComponentManager::EachNew(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.newEntities) + for (const Entity entity : view->NewEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentData(entity))) { break; } @@ -430,14 +448,14 @@ void EntityComponentManager::EachNew(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.newEntities) + for (const Entity entity : view->NewEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -451,14 +469,14 @@ void EntityComponentManager::EachRemoved(typename identityFindView(); + auto view = this->FindView(); // Iterate over the entities in the view and in the newly created // entities list, and invoke the callback // function. - for (const Entity entity : view.toRemoveEntities) + for (const Entity entity : view->ToRemoveEntities()) { - if (!_f(entity, view.Component(entity, this)...)) + if (!std::apply(_f, view->EntityComponentConstData(entity))) { break; } @@ -466,93 +484,71 @@ void EntityComponentManager::EachRemoved(typename identity::type> -void EntityComponentManager::AddComponentsToView(detail::View &_view, - const Entity _entity) const +template +detail::View *EntityComponentManager::FindView() const { - const ComponentTypeId typeId = FirstComponent::typeId; + auto viewKey = std::vector{ComponentTypeTs::typeId...}; - const ComponentId compId = - this->EntityComponentIdFromType(_entity, typeId); - if (compId >= 0) + auto baseViewMutexPair = this->FindView(viewKey); + auto baseViewPtr = baseViewMutexPair.first; + if (nullptr != baseViewPtr) { - // Add the component to the view. - _view.AddComponent(_entity, typeId, compId); - } - else - { - ignerr << "Entity[" << _entity << "] has no component of type[" - << typeId << "]. This should never happen.\n"; - } -} + auto view = static_cast*>(baseViewPtr); -////////////////////////////////////////////////// -template::type> -void EntityComponentManager::AddComponentsToView(detail::View &_view, - const Entity _entity) const -{ - const ComponentTypeId typeId = FirstComponent::typeId; - const ComponentId compId = - this->EntityComponentIdFromType(_entity, typeId); - if (compId >= 0) - { - // Add the component to the view. - _view.AddComponent(_entity, typeId, compId); - } - else - { - ignerr << "Entity[" << _entity << "] has no component of type[" - << typeId << "]. This should never happen.\n"; - } + std::unique_ptr> viewLock; + if (this->LockAddingEntitiesToViews()) + { + // lock the mutex unique to this view in order to prevent multiple threads + // from concurrently reading/modifying the view's toAddEntities data + // (for example, this is useful in system PostUpdates since they are run + // in parallel) + auto mutexPtr = baseViewMutexPair.second; + if (nullptr == mutexPtr) + { + ignerr << "Internal error: requested to lock a view, but no mutex " + << "exists for this view. This should never happen!" << std::endl; + return view; + } + viewLock = std::make_unique>(*mutexPtr); + } - // Add the remaining components to the view. - this->AddComponentsToView(_view, _entity); -} + // add any new entities to the view before using it + for (const auto &[entity, isNew] : view->ToAddEntities()) + { + view->AddEntityWithConstComps(entity, isNew, + this->Component(entity)...); + view->AddEntityWithComps(entity, isNew, + const_cast(this)->Component( + entity)...); + } + view->ClearToAddEntities(); -////////////////////////////////////////////////// -template -detail::View &EntityComponentManager::FindView() const -{ - auto types = std::set{ComponentTypeTs::typeId...}; + return view; + } - std::map::iterator viewIter; + // create a new view if one wasn't found + detail::View view; - // Find the view. If the view doesn't exist, then create a new view. - if (!this->FindView(types, viewIter)) + for (const auto &vertex : this->Entities().Vertices()) { - detail::View view; - // Add all the entities that match the component types to the - // view. - for (const auto &vertex : this->Entities().Vertices()) - { - Entity entity = vertex.first; - if (this->EntityMatches(entity, types)) - { - view.AddEntity(entity, this->IsNewEntity(entity)); - // If there is a request to delete this entity, update the view as - // well - if (this->IsMarkedForRemoval(entity)) - { - view.AddEntityToRemoved(entity); - } - - // Store pointers to all the components. This recursively adds - // all the ComponentTypeTs that belong to the entity to the view. - this->AddComponentsToView(view, entity); - } - } + Entity entity = vertex.first; + + // only add entities to the view that have all of the components in viewKey + if (!this->EntityMatches(entity, view.ComponentTypes())) + continue; - // Store the view. - return this->AddView(types, std::move(view))->second; + view.AddEntityWithConstComps(entity, this->IsNewEntity(entity), + this->Component(entity)...); + view.AddEntityWithComps(entity, this->IsNewEntity(entity), + const_cast(this)->Component( + entity)...); + if (this->IsMarkedForRemoval(entity)) + view.MarkEntityToRemove(entity); } - return viewIter->second; + baseViewPtr = this->AddView(viewKey, + std::make_unique>(view)); + return static_cast*>(baseViewPtr); } ////////////////////////////////////////////////// diff --git a/include/ignition/gazebo/detail/View.hh b/include/ignition/gazebo/detail/View.hh index 654a146291e..93ea32c1b12 100644 --- a/include/ignition/gazebo/detail/View.hh +++ b/include/ignition/gazebo/detail/View.hh @@ -17,14 +17,16 @@ #ifndef IGNITION_GAZEBO_DETAIL_VIEW_HH_ #define IGNITION_GAZEBO_DETAIL_VIEW_HH_ -#include -#include -#include +#include +#include +#include #include -#include "ignition/gazebo/components/Component.hh" + +#include + #include "ignition/gazebo/Entity.hh" -#include "ignition/gazebo/Export.hh" -#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/detail/BaseView.hh" namespace ignition { @@ -34,105 +36,297 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace detail { -/// \brief A key into the map of views -using ComponentTypeKey = std::set; - -/// \brief A view is a cache to entities, and their components, that -/// match a set of component types. A cache is used because systems will -/// frequently, potentially every iteration, query the -/// EntityComponentManager for sets of entities that match a set of -/// component types. Rather than look up the entities every time, we can -/// use a cache to improve performance. The assumption is that entities -/// and the types of components assigned to entities change infrequently -/// compared to the frequency of queries performed by systems. -class IGNITION_GAZEBO_VISIBLE View +/// \brief A view that caches a particular set of component type data. +/// \tparam ComponentTypeTs The component type(s) that are stored in this view. +template +class View : public BaseView { - /// Get a pointer to a component for an entity based on a component type. - /// \param[in] _entity The entity. - /// \param[in] _ecm Pointer to the entity component manager. - /// \return Pointer to the component. - public: template - const ComponentTypeT *Component(const Entity _entity, - const EntityComponentManager *_ecm) const - { - ComponentTypeId typeId = ComponentTypeT::typeId; - return static_cast( - this->ComponentImplementation(_entity, typeId, _ecm)); - } + /// \brief Alias for containers that hold and entity and its component data. + /// The component types held in this container match the component types that + /// were specified when creating the view. + private: using ComponentData = std::tuple; + private: using ConstComponentData = + std::tuple; - /// Get a pointer to a component for an entity based on a component type. - /// \param[in] _entity The entity. - /// \param[in] _ecm Pointer to the entity component manager. - /// \return Pointer to the component. - public: template - ComponentTypeT *Component(const Entity _entity, - const EntityComponentManager *_ecm) - { - ComponentTypeId typeId = ComponentTypeT::typeId; - return static_cast( - const_cast( - this->ComponentImplementation(_entity, typeId, _ecm))); - } + /// \brief Constructor + public: View(); + + /// \brief Documentation inherited + public: bool HasCachedComponentData(const Entity _entity) const override; + + /// \brief Documentation inherited + public: bool RemoveEntity(const Entity _entity) override; + + /// \brief Get an entity and its component data. It is assumed that the entity + /// being requested exists in the view. + /// \param[_in] _entity The entity + /// \return The entity and its component data. Const pointers to the component + /// data are returned. + public: ConstComponentData EntityComponentConstData( + const Entity _entity) const; - /// \brief Add an entity to the view. - /// \param[in] _entity The entity to add. + /// \brief Get an entity and its component data. It is assumed that the entity + /// being requested exists in the view. + /// \param[_in] _entity The entity + /// \return The entity and its component data. Mutable pointers to the + /// component data are returned. + public: ComponentData EntityComponentData(const Entity _entity); + + /// \brief Add an entity with its component data to the view. It is assunmed + /// that the entity to be added does not already exist in the view. + /// \param[in] _entity The entity /// \param[in] _new Whether to add the entity to the list of new entities. /// The new here is to indicate whether the entity is new to the entity /// component manager. An existing entity can be added when creating a new /// view or when rebuilding the view. - public: void AddEntity(const Entity _entity, const bool _new = false); - - /// \brief Remove an entity from the view. - /// \param[in] _entity The entity to remove. - /// \param[in] _key Components that should also be removed. - /// \return True if the entity was removed, false if the entity did not - /// exist in the view. - public: bool RemoveEntity(const Entity _entity, - const ComponentTypeKey &_key); - - /// \brief Add the entity to the list of entities to be removed - /// \param[in] _entity The entity to add. - /// \return True if the entity was added to the list, false if the entity - /// did not exist in the view. - public: bool AddEntityToRemoved(const Entity _entity); - - /// \brief Add a component to an entity. - /// \param[in] _entity The entity. - /// \param[in] _compTypeId Component type id. - /// \param[in] _compId Component id. - public: void AddComponent(const Entity _entity, - const ComponentTypeId _compTypeId, - const ComponentId _compId); - - /// \brief Implementation of the Component accessor. - /// \param[in] _entity The entity. - /// \param[in] _typeId Type id of the component. - /// \param[in] _ecm Pointer to the EntityComponentManager. - /// \return Pointer to the component, or nullptr if not found. - private: const components::BaseComponent *ComponentImplementation( - const Entity _entity, - ComponentTypeId _typeId, - const EntityComponentManager *_ecm) const; - - /// \brief Clear the list of new entities - public: void ClearNewEntities(); - - /// \brief All the entities that belong to this view. - public: std::set entities; - - /// \brief List of newly created entities - public: std::set newEntities; - - /// \brief List of entities about to be removed - public: std::set toRemoveEntities; - - /// \brief All of the components for each entity. - public: std::map, - ComponentId> components; + /// \param[in] _compPtrs Const pointers to the entity's components + public: void AddEntityWithConstComps(const Entity &_entity, const bool _new, + const ComponentTypeTs*... _compPtrs); + + /// \brief Add an entity with its component data to the view. It is assunmed + /// that the entity to be added does not already exist in the view. + /// \param[in] _entity The entity + /// \param[in] _new Whether to add the entity to the list of new entities. + /// The new here is to indicate whether the entity is new to the entity + /// component manager. An existing entity can be added when creating a new + /// view or when rebuilding the view. + /// \param[in] _compPtrs Pointers to the entity's components + public: void AddEntityWithComps(const Entity &_entity, const bool _new, + ComponentTypeTs*... _compPtrs); + + /// \brief Documentation inherited + public: bool NotifyComponentAddition(const Entity _entity, bool _newEntity, + const ComponentTypeId _typeId) override; + + /// \brief Documentation inherited + public: bool NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) override; + + /// \brief Documentation inherited + public: void Reset() override; + + /// \brief A map of entities to their component data. Since tuples are defined + /// at compile time, we need seperate containers that have tuples for both + /// non-const and const component pointers (calls to ECM::Each can have a + /// method signature that uses either non-const or const pointers) + private: std::unordered_map validData; + private: std::unordered_map validConstData; + + /// \brief A map of invalid entities to their component data. The difference + /// between invalidData and validData is that the entities in invalidData were + /// once in validData, but they had a component removed, so the entity no + /// longer meets the component requirements of the view. If the missing + /// component data is ever added back to an entitiy in invalidData, then this + /// entity will be moved back to validData. The usage of invalidData is an + /// implementation detail that should be ignored by those using the View API; + /// from a user's point of view, entities that belong to invalidData don't + /// appear to be a part of the view at all. + /// + /// The reason for moving entities with missing components to invalidData + /// instead of completely deleting them from the view is because if components + /// are added back later and the entity needs to be re-added to the view, + /// tuple creation can be costly. So, this approach is used instead to + /// maintain runtime performance (the tradeoff of mainting performance is + /// increased complexity and memory usage). + /// + /// \sa missingCompTracker + private: std::unordered_map invalidData; + private: std::unordered_map invalidConstData; + + /// \brief A map that keeps track of which component types for entities in + /// invalidData need to be added back to the entity in order to move the + /// entity back to validData. If the set of types (value in the map) becomes + /// empty, then this means that the entity (key in the map) has all of the + /// component types defined by the view, so the entity can be moved back to + /// validData. + /// + /// \sa invalidData + private: std::unordered_map> + missingCompTracker; }; -/// \endcond + +////////////////////////////////////////////////// +template +View::View() +{ + this->componentTypes = {ComponentTypeTs::typeId...}; } + +////////////////////////////////////////////////// +template +bool View::HasCachedComponentData( + const Entity _entity) const +{ + auto cachedComps = + this->validData.find(_entity) != this->validData.end() || + this->invalidData.find(_entity) != this->invalidData.end(); + auto cachedConstComps = + this->validConstData.find(_entity) != this->validConstData.end() || + this->invalidConstData.find(_entity) != this->invalidConstData.end(); + + if (cachedComps && !cachedConstComps) + { + ignwarn << "Non-const component data is cached for entity " << _entity + << ", but const component data is not cached." << std::endl; + } + else if (cachedConstComps && !cachedComps) + { + ignwarn << "Const component data is cached for entity " << _entity + << ", but non-const component data is not cached." << std::endl; + } + + return cachedComps && cachedConstComps; } + +////////////////////////////////////////////////// +template +bool View::RemoveEntity(const Entity _entity) +{ + this->invalidData.erase(_entity); + this->invalidConstData.erase(_entity); + this->missingCompTracker.erase(_entity); + + if (!this->HasEntity(_entity) && !this->IsEntityMarkedForAddition(_entity)) + return false; + + this->entities.erase(_entity); + this->newEntities.erase(_entity); + this->toRemoveEntities.erase(_entity); + this->toAddEntities.erase(_entity); + this->validData.erase(_entity); + this->validConstData.erase(_entity); + + return true; +} + +////////////////////////////////////////////////// +template +typename View::ConstComponentData + View::EntityComponentConstData(const Entity _entity) const +{ + return this->validConstData.at(_entity); +} + +////////////////////////////////////////////////// +template +typename View::ComponentData + View::EntityComponentData(const Entity _entity) +{ + return this->validData.at(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithConstComps(const Entity &_entity, + const bool _new, const ComponentTypeTs*... _compPtrs) +{ + this->validConstData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +void View::AddEntityWithComps(const Entity &_entity, + const bool _new, ComponentTypeTs*... _compPtrs) +{ + this->validData[_entity] = std::make_tuple(_entity, _compPtrs...); + this->entities.insert(_entity); + if (_new) + this->newEntities.insert(_entity); +} + +////////////////////////////////////////////////// +template +bool View::NotifyComponentAddition(const Entity _entity, + bool _newEntity, const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // remove the newly added component type from the missing component types + // list + auto missingCompsIter = this->missingCompTracker.find(_entity); + if (missingCompsIter == this->missingCompTracker.end()) + { + // the component is already added, so nothing else needs to be done + return true; + } + missingCompsIter->second.erase(_typeId); + + // if the entity now has all components that meet the requirements of the + // view, then add the entity back to the view + if (missingCompsIter->second.empty()) + { + auto nh = this->invalidData.extract(_entity); + this->validData.insert(std::move(nh)); + auto constCompNh = this->invalidConstData.extract(_entity); + this->validConstData.insert(std::move(constCompNh)); + this->entities.insert(_entity); + if (_newEntity) + this->newEntities.insert(_entity); + this->missingCompTracker.erase(_entity); + } + + return true; } + +////////////////////////////////////////////////// +template +bool View::NotifyComponentRemoval(const Entity _entity, + const ComponentTypeId _typeId) +{ + // make sure that _typeId is a type required by the view and that _entity is + // already a part of the view + if (!this->RequiresComponent(_typeId) || + !this->HasCachedComponentData(_entity)) + return false; + + // if the component being removed is the first component that causes _entity + // to be invalid for this view, move _entity from validData to invalidData + // since _entity should no longer be considered a part of the view + auto it = this->validData.find(_entity); + auto constCompIt = this->validConstData.find(_entity); + if (it != this->validData.end() && + constCompIt != this->validConstData.end()) + { + auto nh = this->validData.extract(it); + this->invalidData.insert(std::move(nh)); + auto constCompNh = this->validConstData.extract(constCompIt); + this->invalidConstData.insert(std::move(constCompNh)); + this->entities.erase(_entity); + this->newEntities.erase(_entity); + } + + this->missingCompTracker[_entity].insert(_typeId); + + return true; +} + +////////////////////////////////////////////////// +template +void View::Reset() +{ + // reset all data structures in the BaseView except for componentTypes since + // the view always requires the types in componentTypes + this->entities.clear(); + this->newEntities.clear(); + this->toRemoveEntities.clear(); + this->toAddEntities.clear(); + + // reset all data structures unique to the templated view + this->validData.clear(); + this->validConstData.clear(); + this->invalidData.clear(); + this->invalidConstData.clear(); + this->missingCompTracker.clear(); } +} // namespace detail +} // namespace IGNITION_GAZEBO_VERSION_NAMESPACE +} // namespace gazebo +} // namespace ignition #endif diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index e23f99cfead..39cb1f641c2 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -41,8 +41,10 @@ namespace gui /// ign-tools. Set to the name of the application if using ign-tools) /// \param[in] _guiConfig The GUI configuration file. If nullptr, the default /// configuration from IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. - IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, char **_argv, - const char *_guiConfig); + /// \param[in] _renderEngineGui --render-engine-gui option + /// \return -1 on failure, 0 on success + IGNITION_GAZEBO_GUI_VISIBLE int runGui(int &_argc, + char **_argv, const char *_guiConfig, const char * _renderEngine = nullptr); /// \brief Create a Gazebo GUI application /// \param[in] _argc Number of command line arguments (Used when running @@ -60,10 +62,13 @@ namespace gui /// IGN_HOMEDIR/.ignition/gazebo/gui.config will be used. /// \param[in] _loadPluginsFromSdf If true, plugins specified in the world /// SDFormat file will get loaded. + /// \param[in] _renderEngineGui --render-engine-gui option + /// \return Newly created application. IGNITION_GAZEBO_GUI_VISIBLE std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, - const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true); + const char *_defaultGuiConfig = nullptr, bool _loadPluginsFromSdf = true, + const char *_renderEngine = nullptr); } // namespace gui } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 20002bd6b1d..8bac2759934 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -18,10 +18,13 @@ #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_ #include +#include #include #include #include #include +#include +#include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -36,58 +39,8 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// more information about events. namespace events { - /// \brief The class for sending and receiving custom snap value events. - class IGN_DEPRECATED(5) SnapIntervals : public QEvent - { - /// \brief Constructor - /// \param[in] _xyz XYZ snapping values. - /// \param[in] _rpy RPY snapping values. - /// \param[in] _scale Scale snapping values. - public: SnapIntervals( - const math::Vector3d &_xyz, - const math::Vector3d &_rpy, - const math::Vector3d &_scale) - : QEvent(kType), xyz(_xyz), rpy(_rpy), scale(_scale) - { - } - - /// \brief Get the XYZ snapping values. - /// \return The XYZ snapping values. - public: math::Vector3d XYZ() const - { - return this->xyz; - } - - /// \brief Get the RPY snapping values. - /// \return The RPY snapping values. - public: math::Vector3d RPY() const - { - return this->rpy; - } - - /// \brief Get the scale snapping values. - /// \return The scale snapping values. - public: math::Vector3d Scale() const - { - return this->scale; - } - - /// \brief The QEvent representing a snap event occurrence. - static const QEvent::Type kType = QEvent::Type(QEvent::User); - - /// \brief XYZ snapping values in meters, these values must be positive. - private: math::Vector3d xyz; - - /// \brief RPY snapping values in degrees, these values must be positive. - private: math::Vector3d rpy; - - /// \brief Scale snapping values - a multiplier of the current size, - /// these values must be positive. - private: math::Vector3d scale; - }; - /// \brief Event that notifies when new entities have been selected. - class EntitiesSelected : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE EntitiesSelected : public QEvent { /// \brief Constructor /// \param[in] _entities All the selected entities @@ -125,7 +78,7 @@ namespace events }; /// \brief Event that notifies when all entities have been deselected. - class DeselectAllEntities : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE DeselectAllEntities : public QEvent { /// \brief Constructor /// \param[in] _fromUser True if the event was directly generated by the @@ -149,66 +102,79 @@ namespace events private: bool fromUser{false}; }; - /// \brief Event called in the render thread of a 3D scene. - /// It's safe to make rendering calls in this event's callback. - class IGN_DEPRECATED(5) Render : public QEvent + /// \brief Event that contains entities newly created or removed from the + /// GUI, but that aren't present on the server yet. + /// \sa NewRemovedEntities + class IGNITION_GAZEBO_GUI_VISIBLE GuiNewRemovedEntities : public QEvent { - public: Render() - : QEvent(kType) - { - } + /// \brief Constructor + /// \param[in] _newEntities Set of newly created entities + /// \param[in] _removedEntities Set of recently removed entities + public: GuiNewRemovedEntities(const std::set &_newEntities, + const std::set &_removedEntities); + + /// \brief Get the set of newly created entities + public: const std::set &NewEntities() const; + + /// \brief Get the set of recently removed entities + public: const std::set &RemovedEntities() const; + /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief Event called to spawn a preview model. - /// Used by plugins that spawn models. - class IGN_DEPRECATED(5) SpawnPreviewModel : public QEvent + /// \brief Event that notifies when new entities have been created or removed + /// on the server. This is a duplication of what `GuiSystem`s would get from + /// `EachNew` / `EachRemoved` ECM calls. + /// \sa GuiNewRemovedEntities + class IGNITION_GAZEBO_GUI_VISIBLE NewRemovedEntities : public QEvent { /// \brief Constructor - /// \param[in] _modelSdfString The model's SDF file as a string. - public: explicit SpawnPreviewModel(const std::string &_modelSdfString) - : QEvent(kType), modelSdfString(_modelSdfString) - { - } + /// \param[in] _newEntities Set of newly created entities + /// \param[in] _removedEntities Set of recently removed entities + public: NewRemovedEntities(const std::set &_newEntities, + const std::set &_removedEntities); + + /// \brief Get the set of newly created entities + public: const std::set &NewEntities() const; + + /// \brief Get the set of recently removed entities + public: const std::set &RemovedEntities() const; /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 4); - /// \brief Get the sdf string of the model. - /// \return The model sdf string - public: std::string ModelSdfString() const - { - return this->modelSdfString; - } - - /// \brief The sdf string of the model to be previewed. - std::string modelSdfString; + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief Event called to spawn a preview resource, which takes the path - /// to the SDF file. Used by plugins that spawn resources. - class IGN_DEPRECATED(5) SpawnPreviewPath : public QEvent + /// \brief True if a transform control is currently active (translate / + /// rotate / scale). False if we're in selection mode. + class IGNITION_GAZEBO_GUI_VISIBLE TransformControlModeActive : public QEvent { /// \brief Constructor - /// \param[in] _filePath The path to an SDF file. - public: explicit SpawnPreviewPath(const std::string &_filePath) - : QEvent(kType), filePath(_filePath) + /// \param[in] _tranformModeActive is the transform control mode active + public: explicit TransformControlModeActive(const bool _tranformModeActive) + : QEvent(kType), tranformModeActive(_tranformModeActive) { } /// \brief Unique type for this event. - static const QEvent::Type kType = QEvent::Type(QEvent::User + 5); + static const QEvent::Type kType = QEvent::Type(QEvent::User + 6); - /// \brief Get the path of the SDF file. - /// \return The file path. - public: std::string FilePath() const + /// \brief Get the event's value. + public: bool TransformControlActive() { - return this->filePath; + return this->tranformModeActive; } - /// \brief The path of SDF file to be previewed. - std::string filePath; + /// \brief True if a transform mode is active. + private: bool tranformModeActive; }; } // namespace events } diff --git a/include/ignition/gazebo/gui/TmpIface.hh b/include/ignition/gazebo/gui/TmpIface.hh deleted file mode 100644 index c963505a093..00000000000 --- a/include/ignition/gazebo/gui/TmpIface.hh +++ /dev/null @@ -1,80 +0,0 @@ -/* - * 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. - * -*/ -#ifndef IGNITION_GAZEBO_GUI_TMPIFACE_HH_ -#define IGNITION_GAZEBO_GUI_TMPIFACE_HH_ - -#ifndef Q_MOC_RUN - #include -#endif - -#include -#include - -#include "ignition/gazebo/gui/Export.hh" - -namespace ignition -{ - namespace gazebo - { - /// \brief Temporary place to prototype transport interfaces while it's not - /// clear where they will live. - /// - /// Move API from here to their appropriate locations once that's known. - /// - /// This class should be removed before releasing! - class TmpIface : public QObject - { - Q_OBJECT - - /// \brief Constructor: advertize services and topics - public: IGN_DEPRECATED(5.0) TmpIface(); - - /// \brief Destructor - public: ~TmpIface() override = default; - - /// \brief Callback when user asks to start a new world. - /// This is the client-side logic which requests the server_control - /// service. - public slots: void OnNewWorld(); - - /// \brief Callback when user asks to load a world file. - /// This is the client-side logic which requests the server_control - /// service. - /// \param[in] _path Path to world file. - public slots: void OnLoadWorld(const QString &_path); - - /// \brief Callback when user asks to save a world file providing a path. - /// This is the client-side logic which requests the server_control - /// service. - /// \param[in] _path Path to world file. - public slots: void OnSaveWorldAs(const QString &_path); - - /// \brief Server control service callback - /// This is the server-side logic which provides the world_control - /// service. - /// \param[in] _req Request - /// \param[out] _res Response - /// \return True for success - private: bool OnServerControl(const msgs::ServerControl &_req, - msgs::Boolean &_res); - - /// \brief Communication node - private: transport::Node node; - }; - } -} -#endif diff --git a/include/ignition/gazebo/physics/Events.hh b/include/ignition/gazebo/physics/Events.hh new file mode 100644 index 00000000000..668e19a2ad7 --- /dev/null +++ b/include/ignition/gazebo/physics/Events.hh @@ -0,0 +1,65 @@ +/* + * 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_PHYSICS_EVENTS_HH_ +#define IGNITION_GAZEBO_PHYSICS_EVENTS_HH_ + +#include + +#include + +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/Entity.hh" + +#include + +namespace ignition +{ + namespace gazebo + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + + namespace events + { + using Policy = physics::FeaturePolicy3d; + + /// \brief This event is called when the physics engine needs to collect + /// what customizations it should do to the surface of a contact point. It + /// is called during the Update phase after collision checking has been + /// finished and before the physics update has happened. The event + /// subscribers are expected to change the `params` argument. + using CollectContactSurfaceProperties = ignition::common::EventT< + void( + const Entity& /* collision1 */, + const Entity& /* collision2 */, + const math::Vector3d & /* point */, + const std::optional /* force */, + const std::optional /* normal */, + const std::optional /* depth */, + const size_t /* numContactsOnCollision */, + physics::SetContactPropertiesCallbackFeature:: + ContactSurfaceParams& /* params */ + ), + struct CollectContactSurfacePropertiesTag>; + } + } // namespace events + } // namespace gazebo +} // namespace ignition + +#endif // IGNITION_GAZEBO_PHYSICS_EVENTS_HH_ diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 8f21ff3625e..9a36adcc55b 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -84,6 +84,14 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Name of the rendering engine public: std::string EngineName() const; + /// \brief Set the headless mode + /// \param[in] _headless Set to true to enable headless mode. + public: void SetHeadlessRendering(const bool &_headless); + + /// \brief Get the headless mode + /// \return True if headless mode is enable, false otherwise. + public: bool HeadlessRendering() const; + /// \brief Set the scene to use /// \param[in] _sceneName Name of the engine. public: void SetSceneName(const std::string &_sceneName); @@ -92,11 +100,17 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Name of the rendering scene. public: std::string SceneName() const; - /// \brief Set background color of render window + /// \brief Set the scene to use. + /// \param[in] _scene Pointer to the scene. + public: void SetScene(const rendering::ScenePtr &_scene); + + /// \brief Set background color of render window. This will override + /// other sources, such as from SDF. /// \param[in] _color Color of render window background public: void SetBackgroundColor(const math::Color &_color); - /// \brief Set ambient light of render window + /// \brief Set ambient light of render window. This will override + /// other sources, such as from SDF. /// \param[in] _ambient Color of ambient light public: void SetAmbientLight(const math::Color &_ambient); @@ -111,6 +125,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _enable True to use the current GL context public: void SetUseCurrentGLContext(bool _enable); + /// \brief Set the Window ID + /// \param[in] _winID Window ID + public: void SetWinID(const std::string &_winID); + /// \brief Set whether to create rendering sensors /// \param[in] _enable True to create rendering sensors /// \param[in] _createSensorCb Callback function for creating the sensors @@ -126,6 +144,26 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public : void SetRemoveSensorCb( std::function _removeSensorCb); + /// \brief View an entity as transparent + /// \param[in] _entity Entity to view as transparent + public: void ViewTransparent(const Entity &_entity); + + /// \brief View center of mass of specified entity + /// \param[in] _entity Entity to view center of mass + public: void ViewCOM(const Entity &_entity); + + /// \brief View inertia of specified entity + /// \param[in] _entity Entity to view inertia + public: void ViewInertia(const Entity &_entity); + + /// \brief View joints of specified entity + /// \param[in] _entity Entity to view joints + public: void ViewJoints(const Entity &_entity); + + /// \brief View wireframes of specified entity + /// \param[in] _entity Entity to view wireframes + public: void ViewWireframes(const Entity &_entity); + /// \brief View collisions of specified entity which are shown in orange /// \param[in] _entity Entity to view collisions public: void ViewCollisions(const Entity &_entity); @@ -147,10 +185,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _node Node representing the selected entity public: void SetSelectedEntity(const rendering::NodePtr &_node); - /// \brief Get the entity being selected. This will only return the - /// last entity selected. - public: rendering::NodePtr IGN_DEPRECATED(4) SelectedEntity() const; - /// \brief Get the entities currently selected, in order of selection. /// \return Vector of currently selected entities public: const std::vector &SelectedEntities() const; @@ -158,6 +192,15 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Clears the set of selected entities and lowlights all of them. public: void DeselectAllEntities(); + /// \brief Helper function to get all child links of a model entity. + /// \param[in] _entity Entity to find child links + /// \return Vector of child links found for the parent entity + private: std::vector FindChildLinks(const Entity &_entity); + + /// \brief Helper function to hide wireboxes for an entity + /// \param[in] _entity Entity to hide wireboxes + private: void HideWireboxes(const Entity &_entity); + /// \brief Set whether the transform controls are currently being dragged. /// \param[in] _active True if active. public: void SetTransformActive(bool _active); diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index 248a648735f..73f890302a3 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -21,9 +21,12 @@ #include #include #include +#include +#include #include #include +#include #include #include #include @@ -126,6 +129,30 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateLink(Entity _id, const sdf::Link &_link, Entity _parentId = 0); + /// \brief Filter a node and its children according to specific criteria. + /// \param[in] _node The name of the node where filtering should start. + /// \param[in] _filter Callback function that defines how _node and its + /// children should be filtered. The function parameter is a node. The + /// callback returns true if the node should be filtered; false otherwise. + /// \return A list of filtered nodes in top level order. This list can + /// contain _node itself, or child nodes of _node. An empty list means no + /// nodes were filtered. + public: std::vector Filter(const std::string &_node, + std::function _filter) const; + + /// \brief Copy a visual that currently exists in the scene + /// \param[in] _id Unique visual id of the copied visual + /// \param[in] _visual Name of the visual to copy + /// \param[in] _parentId Parent id of the copied visual + /// \return A pair with the first element being the copied visual object, + /// and the second element being a list of the entity IDs for the copied + /// visual's children, in level order. If copying the visual failed, the + /// first element will be nullptr. If the copied visual has no children, the + /// second element will be empty. + public: std::pair> CopyVisual( + Entity _id, const std::string &_visual, Entity _parentId = 0); + /// \brief Create a visual /// \param[in] _id Unique visual id /// \param[in] _visual Visual sdf dom @@ -134,6 +161,32 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::VisualPtr CreateVisual(Entity _id, const sdf::Visual &_visual, Entity _parentId = 0); + /// \brief Create a center of mass visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parentId Parent id + /// \return Visual (center of mass) object created from the inertial + public: rendering::VisualPtr CreateCOMVisual(Entity _id, + const math::Inertiald &_inertial, Entity _parentId = 0); + + /// \brief Create an inertia visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parentId Parent id + /// \return Visual (inertia) object created from the inertial + public: rendering::VisualPtr CreateInertiaVisual(Entity _id, + const math::Inertiald &_inertial, Entity _parentId = 0); + + /// \brief Create a joint visual + /// \param[in] _id Unique visual id + /// \param[in] _joint Joint sdf dom + /// \param[in] _childId Joint child id + /// \param[in] _parentId Joint parent id + /// \return Visual (joint) object created from the sdf dom + public: rendering::VisualPtr CreateJointVisual(Entity _id, + const sdf::Joint &_joint, Entity _childId = 0, + Entity _parentId = 0); + /// \brief Create a collision visual /// \param[in] _id Unique visual id /// \param[in] _collision Collision sdf dom @@ -150,26 +203,30 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Create an actor /// \param[in] _id Unique actor id /// \param[in] _actor Actor sdf dom + /// \param[in] _name Actor's name /// \param[in] _parentId Parent id /// \return Actor object created from the sdf dom public: rendering::VisualPtr CreateActor(Entity _id, - const sdf::Actor &_actor, Entity _parentId = 0); + const sdf::Actor &_actor, const std::string &_name, + Entity _parentId = 0); /// \brief Create a light /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom + /// \param[in] _name Light's name /// \param[in] _parentId Parent id /// \return Light object created from the sdf dom public: rendering::LightPtr CreateLight(Entity _id, - const sdf::Light &_light, Entity _parentId); + const sdf::Light &_light, const std::string &_name, Entity _parentId); /// \brief Create a light /// \param[in] _id Unique light id /// \param[in] _light Light sdf dom + /// \param[in] _name Light's name /// \param[in] _parentId Parent id /// \return Light object created from the sdf dom public: rendering::VisualPtr CreateLightVisual(Entity _id, - const sdf::Light &_light, Entity _parentId); + const sdf::Light &_light, const std::string &_name, Entity _parentId); /// \brief Create a particle emitter. /// \param[in] _id Unique particle emitter id @@ -216,17 +273,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \return Pointer to requested entity's skeleton public: common::SkeletonPtr ActorSkeletonById(Entity _id) const; - /// \brief Get the animation of actor mesh given an id - /// Use this function if you are animating the actor manually by its - /// skeleton node pose. - /// \param[in] _id Entity's unique id - /// \param[in] _time Simulation time - /// \return Map from the skeleton node name to transforms - /// \deprecated see ActorSkeletonTransformAt - public: std::map IGN_DEPRECATED(4.0) - ActorMeshAnimationAt( - Entity _id, std::chrono::steady_clock::duration _time) const; - /// \brief Get the skeleton local transforms of actor mesh given an id. /// Use this function if you are animating the actor manually by its /// skeleton node pose. @@ -250,14 +296,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _id Entity's unique id public: void RemoveEntity(Entity _id); - /// \brief Get the entity for a given node. - /// \param[in] _node Node to get the entity for. - /// \return The entity for that node, or `kNullEntity` for no entity. - /// \todo(anyone) Deprecate in favour of - /// `ignition::rendering::Node::UserData` once that's available. - public: Entity IGN_DEPRECATED(4) - EntityFromNode(const rendering::NodePtr &_node) const; - /// \brief Load a geometry /// \param[in] _geom Geometry sdf dom /// \param[out] _scale Geometry scale that will be set based on sdf @@ -289,6 +327,26 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::NodePtr TopLevelNode( const rendering::NodePtr &_node) const; + /// \brief Updates the node to increase its transparency or reset + /// back to its original transparency value, an opaque call requires + /// a previous transparent call, otherwise, no action will be taken + /// Usually, this will be a link visual + /// \param[in] _node The node to update. + /// \param[in] _makeTransparent true if updating to increase transparency, + /// false to set back to original transparency values (make more opaque) + public: void UpdateTransparency(const rendering::NodePtr &_node, + bool _makeTransparent); + + /// \brief Updates the world pose of joint parent visual + /// according to its child. + /// \param[in] _jointId Joint visual id. + public: void UpdateJointParentPose(Entity _jointId); + + /// \brief Create a unique entity ID + /// \return A unique entity ID. kNullEntity is returned if no unique entity + /// IDs are available + public: Entity UniqueId() const; + /// \internal /// \brief Pointer to private data class private: std::unique_ptr dataPtr; diff --git a/src/BaseView.cc b/src/BaseView.cc new file mode 100644 index 00000000000..23e78ee607d --- /dev/null +++ b/src/BaseView.cc @@ -0,0 +1,109 @@ +/* + * 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 "ignition/gazebo/detail/BaseView.hh" + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" + +using namespace ignition; +using namespace gazebo; +using namespace detail; + +////////////////////////////////////////////////// +bool BaseView::HasEntity(const Entity _entity) const +{ + return this->entities.find(_entity) != this->entities.end(); +} + +////////////////////////////////////////////////// +bool BaseView::IsEntityMarkedForAddition(const Entity _entity) const +{ + return this->toAddEntities.find(_entity) != this->toAddEntities.end(); +} + +////////////////////////////////////////////////// +bool BaseView::MarkEntityToAdd(const Entity _entity, bool _new) +{ + if (this->HasCachedComponentData(_entity)) + return false; + + this->toAddEntities[_entity] = _new; + return true; +} + +////////////////////////////////////////////////// +bool BaseView::RequiresComponent(const ComponentTypeId _typeId) const +{ + return this->componentTypes.find(_typeId) != this->componentTypes.end(); +} + +////////////////////////////////////////////////// +bool BaseView::MarkEntityToRemove(const Entity _entity) +{ + if (this->HasCachedComponentData(_entity) || + this->IsEntityMarkedForAddition(_entity)) + { + this->toRemoveEntities.insert(_entity); + return true; + } + return false; +} + +////////////////////////////////////////////////// +void BaseView::ResetNewEntityState() +{ + this->newEntities.clear(); + + // mark all entities in the toAddEntities map as not newly created + for (auto &entityNewPair : this->toAddEntities) + entityNewPair.second = false; +} + +////////////////////////////////////////////////// +const std::set &BaseView::ComponentTypes() const +{ + return this->componentTypes; +} + +const std::set &BaseView::Entities() const +{ + return this->entities; +} + +////////////////////////////////////////////////// +const std::set &BaseView::NewEntities() const +{ + return this->newEntities; +} + +////////////////////////////////////////////////// +const std::set &BaseView::ToRemoveEntities() const +{ + return this->toRemoveEntities; +} + +////////////////////////////////////////////////// +const std::unordered_map &BaseView::ToAddEntities() const +{ + return this->toAddEntities; +} + +////////////////////////////////////////////////// +void BaseView::ClearToAddEntities() +{ + this->toAddEntities.clear(); +} diff --git a/src/BaseView_TEST.cc b/src/BaseView_TEST.cc new file mode 100644 index 00000000000..855168266db --- /dev/null +++ b/src/BaseView_TEST.cc @@ -0,0 +1,519 @@ +/* + * 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 "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/Types.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/detail/BaseView.hh" +#include "ignition/gazebo/detail/View.hh" + +#include "../test/helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class BaseViewTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentTypes) +{ + auto modelNameView = detail::View(); + + // make sure that a view's required component types are initialized properly + EXPECT_EQ(2u, modelNameView.ComponentTypes().size()); + EXPECT_NE(modelNameView.ComponentTypes().find(components::Model::typeId), + modelNameView.ComponentTypes().end()); + EXPECT_NE(modelNameView.ComponentTypes().find(components::Name::typeId), + modelNameView.ComponentTypes().end()); + EXPECT_TRUE(modelNameView.RequiresComponent(components::Model::typeId)); + EXPECT_TRUE(modelNameView.RequiresComponent(components::Name::typeId)); + EXPECT_FALSE(modelNameView.RequiresComponent(components::Visual::typeId)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ToAddEntities) +{ + auto modelNameView = detail::View(); + const Entity e1 = 1; + auto e1IsNew = true; + + // by default, the view shouldn't have any entities marked as ToAdd + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + + // Calling ClearToAddEntities should have no effect since no entities are + // marked as ToAdd + modelNameView.ClearToAddEntities(); + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + + // Mark an entity as ToAdd (and as a "newly created" entity) + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + auto e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); + + // Try to mark an entity as ToAdd that is already marked as ToAdd. + // This time around, the entity is not flagged as a "newly created" entity + // when the entity is marked as ToAdd, so we should see this update when + // inspecting the entity in the toAddEntities queue + e1IsNew = false; + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); + + // Remove an entity's ToAdd status and then re-mark it as ToAdd + modelNameView.ClearToAddEntities(); + EXPECT_EQ(0u, modelNameView.ToAddEntities().size()); + EXPECT_FALSE(modelNameView.IsEntityMarkedForAddition(e1)); + e1IsNew = true; + EXPECT_TRUE(modelNameView.MarkEntityToAdd(e1, e1IsNew)); + EXPECT_TRUE(modelNameView.IsEntityMarkedForAddition(e1)); + EXPECT_EQ(1u, modelNameView.ToAddEntities().size()); + e1ToAddIter = modelNameView.ToAddEntities().find(e1); + ASSERT_NE(e1ToAddIter, modelNameView.ToAddEntities().end()); + EXPECT_EQ(e1ToAddIter->second, e1IsNew); + // (entities marked as ToAdd aren't considered as entities that are a part + // of the view yet) + EXPECT_FALSE(modelNameView.HasEntity(e1)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, AddEntities) +{ + auto modelNameView = detail::View(); + + // Initially, the view should have no entities + EXPECT_EQ(0u, modelNameView.Entities().size()); + EXPECT_EQ(0u, modelNameView.NewEntities().size()); + + // Create a few entities with components for the view + const Entity e1 = 1; + const auto e1IsNew = false; + auto e1ModelComp = components::Model(); + auto e1NameComp = components::Name("e1"); + + const Entity e2 = 2; + const auto e2IsNew = true; + auto e2ModelComp = components::Model(); + auto e2NameComp = components::Name("e2"); + + // Add the entities and their components to the view. + EXPECT_FALSE(modelNameView.HasEntity(e1)); + EXPECT_FALSE(modelNameView.HasEntity(e2)); + EXPECT_FALSE(modelNameView.HasCachedComponentData(e1)); + EXPECT_FALSE(modelNameView.HasCachedComponentData(e2)); + modelNameView.AddEntityWithComps(e1, e1IsNew, &e1ModelComp, &e1NameComp); + modelNameView.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp, &e1NameComp); + modelNameView.AddEntityWithComps(e2, e2IsNew, &e2ModelComp, &e2NameComp); + modelNameView.AddEntityWithConstComps(e2, e2IsNew, &e2ModelComp, &e2NameComp); + EXPECT_TRUE(modelNameView.HasEntity(e1)); + EXPECT_TRUE(modelNameView.HasEntity(e2)); + EXPECT_TRUE(modelNameView.HasCachedComponentData(e1)); + EXPECT_TRUE(modelNameView.HasCachedComponentData(e2)); + EXPECT_EQ(2u, modelNameView.Entities().size()); + EXPECT_NE(modelNameView.Entities().find(e1), modelNameView.Entities().end()); + EXPECT_NE(modelNameView.Entities().find(e2), modelNameView.Entities().end()); + EXPECT_EQ(1u, modelNameView.NewEntities().size()); + EXPECT_NE(modelNameView.NewEntities().find(e2), + modelNameView.NewEntities().end()); + + auto e1ConstData = modelNameView.EntityComponentConstData(e1); + EXPECT_EQ(e1, std::get(e1ConstData)); + EXPECT_EQ(&e1ModelComp, std::get(e1ConstData)); + EXPECT_EQ(&e1NameComp, std::get(e1ConstData)); + + auto e1Data = modelNameView.EntityComponentData(e1); + EXPECT_EQ(e1, std::get(e1Data)); + EXPECT_EQ(&e1ModelComp, std::get(e1Data)); + EXPECT_EQ(&e1NameComp, std::get(e1Data)); + + auto e2ConstData = modelNameView.EntityComponentConstData(e2); + EXPECT_EQ(e2, std::get(e2ConstData)); + EXPECT_EQ(&e2ModelComp, std::get(e2ConstData)); + EXPECT_EQ(&e2NameComp, std::get(e2ConstData)); + + auto e2Data = modelNameView.EntityComponentData(e2); + EXPECT_EQ(e2, std::get(e2Data)); + EXPECT_EQ(&e2ModelComp, std::get(e2Data)); + EXPECT_EQ(&e2NameComp, std::get(e2Data)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, RemoveEntities) +{ + auto view = detail::View(); + + const Entity e1 = 1; + auto e1ModelComp = components::Model(); + const Entity e2 = 2; + auto e2ModelComp = components::Model(); + const auto isNewEntity = false; + + // add entities to the view + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithComps(e2, isNewEntity, &e2ModelComp); + view.AddEntityWithConstComps(e2, isNewEntity, &e2ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // remove entities from the view + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_TRUE(view.RemoveEntity(e1)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_TRUE(view.RemoveEntity(e2)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + + // try to remove an entity that is not in the view + EXPECT_FALSE(view.RemoveEntity(e1)); + + // add entities to the view's toAdd and toRemove queue + EXPECT_TRUE(view.MarkEntityToAdd(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_EQ(1u, view.ToAddEntities().size()); + EXPECT_NE(view.ToAddEntities().end(), view.ToAddEntities().find(e1)); + EXPECT_EQ(1u, view.ToRemoveEntities().size()); + EXPECT_NE(view.ToRemoveEntities().end(), view.ToRemoveEntities().find(e1)); + + // remove entities e1 and e2 from the view and make sure that the toAdd and + // toRemove queues are updated to no longer have the removed entities + EXPECT_TRUE(view.RemoveEntity(e1)); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, Reset) +{ + auto view = detail::View(); + + // initially, the view should be completely empty, except for its component + // types (the view's component types are defined at object instantiation time + // and never change) + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_EQ(1u, view.ComponentTypes().size()); + EXPECT_NE(view.ComponentTypes().end(), + view.ComponentTypes().find(components::Model::typeId)); + + // populate the view with entity/component information + const auto isNewEntity = true; + const Entity e1 = 1; + auto e1ModelComp = components::Model(); + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + const Entity e2 = 2; + EXPECT_TRUE(view.MarkEntityToAdd(e2)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + + // make sure the view doesn't have empty entity-related data structures + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(1u, view.ToAddEntities().size()); + EXPECT_EQ(1u, view.ToRemoveEntities().size()); + + // reset the view and make sure that it's back in its original state + view.Reset(); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(0u, view.ToAddEntities().size()); + EXPECT_EQ(0u, view.ToRemoveEntities().size()); + EXPECT_EQ(1u, view.ComponentTypes().size()); + EXPECT_NE(view.ComponentTypes().end(), + view.ComponentTypes().find(components::Model::typeId)); + + // add newly created entities to the view + view.AddEntityWithComps(e1, isNewEntity, &e1ModelComp); + view.AddEntityWithConstComps(e1, isNewEntity, &e1ModelComp); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.MarkEntityToAdd(e2, isNewEntity)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(1u, view.ToAddEntities().size()); + auto e2ToAddIter = view.ToAddEntities().find(e2); + ASSERT_NE(view.ToAddEntities().end(), e2ToAddIter); + EXPECT_EQ(e2ToAddIter->second, isNewEntity); + + // reset the newly created entity state of the view + view.ResetNewEntityState(); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.MarkEntityToRemove(e1)); + EXPECT_TRUE(view.IsEntityMarkedForAddition(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_EQ(1u, view.ToAddEntities().size()); + e2ToAddIter = view.ToAddEntities().find(e2); + ASSERT_NE(view.ToAddEntities().end(), e2ToAddIter); + // entity e2 should still be an entity marked as ToAdd, but it shouldn't + // be a newly created entity anymore + EXPECT_FALSE(e2ToAddIter->second); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, CachedComponentData) +{ + auto view = detail::View(); + + const Entity e1 = 1; + const auto e1IsNew = true; + auto e1ModelComp = components::Model(); + + EXPECT_FALSE(view.HasCachedComponentData(e1)); + + // add both const and non-const component data for e1 to the view + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // reset the view and add only const component data this time + view.Reset(); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + + // reset the view and add only non-const component data this time + view.Reset(); + EXPECT_FALSE(view.HasCachedComponentData(e1)); + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp); + EXPECT_FALSE(view.HasCachedComponentData(e1)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentChangeNotification) +{ + auto view = detail::View(); + + const Entity e1 = 1; + const auto e1IsNew = true; + auto e1ModelComp = components::Model(); + auto e1VisualComp = components::Visual(); + + // add the entity and its component data to the view + view.AddEntityWithComps(e1, e1IsNew, &e1ModelComp, &e1VisualComp); + view.AddEntityWithConstComps(e1, e1IsNew, &e1ModelComp, &e1VisualComp); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_NE(view.NewEntities().end(), view.NewEntities().find(e1)); + + // mimic a removal of e1's model component by notifying the view that this + // component was removed + EXPECT_TRUE(view.NotifyComponentRemoval(e1, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // mimic a removal of e1's visual component by notifying the view that this + // component was removed + EXPECT_TRUE(view.NotifyComponentRemoval(e1, components::Visual::typeId)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // mimic re-addition of e1's model component by notifying the view that this + // component was added. At this point, the visual component is still missing, + // so e1 shouldn't be a part of the view yet + EXPECT_TRUE(view.NotifyComponentAddition(e1, e1IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.HasEntity(e1)); + EXPECT_EQ(0u, view.Entities().size()); + EXPECT_EQ(0u, view.NewEntities().size()); + + // mimic re-addition of e1's visual component by notifying the view that this + // component was added. Now that both the model and visual components have + // been re-added to e1, e1 should be a part of the view + EXPECT_TRUE(view.NotifyComponentAddition(e1, e1IsNew, + components::Visual::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_NE(view.NewEntities().end(), view.NewEntities().find(e1)); + + // try to call NotifyComponent* methods with component types that don't + // belong to the view + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + EXPECT_FALSE(view.RequiresComponent(components::Name::typeId)); + EXPECT_FALSE(view.NotifyComponentRemoval(e1, components::Name::typeId)); + EXPECT_FALSE(view.NotifyComponentAddition(e1, e1IsNew, + components::Name::typeId)); + EXPECT_TRUE(view.HasEntity(e1)); + EXPECT_TRUE(view.HasCachedComponentData(e1)); + + // try to call NotifyComponent* methods with entities that aren't a part of + // the view + const Entity e2 = 2; + const auto e2IsNew = false; + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.RequiresComponent(components::Model::typeId)); + EXPECT_FALSE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + + // add another entity to the view that isn't a newly created entity to make + // sure that calling NotifyComponent* methods on this entity doesn't modify + // the view's new entity data + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_FALSE(view.HasCachedComponentData(e2)); + auto e2ModelComp = components::Model(); + auto e2VisualComp = components::Visual(); + view.AddEntityWithComps(e2, e2IsNew, &e2ModelComp, &e2VisualComp); + view.AddEntityWithConstComps(e2, e2IsNew, &e2ModelComp, &e2VisualComp); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); + + // call NotifyComponentRemoval on the entity that was just added to the view + EXPECT_TRUE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // call NotifyComponentRemoval on a component that was already notified of + // removal. While the notification should still take place, it will have no + // effect since this component was already removed + EXPECT_TRUE(view.NotifyComponentRemoval(e2, components::Model::typeId)); + EXPECT_FALSE(view.HasEntity(e2)); + EXPECT_EQ(1u, view.Entities().size()); + EXPECT_EQ(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + + // call NotifyComponentAddition on the entity that was just added to the view + EXPECT_TRUE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); + + // call NotifyComponentAddition on a component that was already notified of + // addition. While the notification should still take place, it will have no + // effect since this component was already added + EXPECT_TRUE(view.NotifyComponentAddition(e2, e2IsNew, + components::Model::typeId)); + EXPECT_TRUE(view.HasCachedComponentData(e2)); + EXPECT_TRUE(view.HasEntity(e2)); + EXPECT_EQ(2u, view.Entities().size()); + EXPECT_NE(view.Entities().end(), view.Entities().find(e1)); + EXPECT_NE(view.Entities().end(), view.Entities().find(e2)); + EXPECT_EQ(1u, view.NewEntities().size()); + EXPECT_EQ(view.NewEntities().end(), view.NewEntities().find(e2)); +} + +///////////////////////////////////////////////// +TEST_F(BaseViewTest, ComponentTypeHasher) +{ + // Test the hash function for a std::vector to make + // sure that views with different component types (either a different type + // ordering or a different set of types alltogether) are considered unique + using ComponentVec = std::vector; + + // Create vectors with the same types, but different order + ComponentVec vec1 = { + components::Model::typeId, + components::Name::typeId, + components::Visual::typeId + }; + ComponentVec vec2 = { + components::Name::typeId, + components::Model::typeId, + components::Visual::typeId + }; + ComponentVec vec3 = { + components::Model::typeId, + components::Visual::typeId, + components::Name::typeId + }; + + // Create vectors with different types + ComponentVec vec4 = {components::Model::typeId}; + ComponentVec vec5 = {components::Name::typeId}; + ComponentVec vec6 = {components::Visual::typeId}; + ComponentVec vec7 = { + components::Model::typeId, + components::Name::typeId + }; + + // Test the hash function. Each vector defined above should be unique, which + // means that the std::unordered_set should have 7 elements in it after all + // insertions have been attempted (7 unique vectors were created) + std::unordered_set uniqueVecs; + uniqueVecs.insert(vec1); + uniqueVecs.insert(vec2); + uniqueVecs.insert(vec3); + uniqueVecs.insert(vec4); + uniqueVecs.insert(vec5); + uniqueVecs.insert(vec6); + uniqueVecs.insert(vec7); + EXPECT_EQ(7u, uniqueVecs.size()); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 4a750222d69..358256f1e3b 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -46,11 +46,13 @@ target_link_libraries(${ign_lib_target} set (sources Barrier.cc + BaseView.cc Conversions.cc EntityComponentManager.cc LevelManager.cc Link.cc Model.cc + Primitives.cc SdfEntityCreator.cc SdfGenerator.cc Server.cc @@ -60,7 +62,6 @@ set (sources SystemLoader.cc TestFixture.cc Util.cc - View.cc World.cc cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} @@ -70,30 +71,49 @@ set (sources set (gtest_sources ${gtest_sources} Barrier_TEST.cc - Component_TEST.cc + BaseView_TEST.cc ComponentFactory_TEST.cc + Component_TEST.cc Conversions_TEST.cc EntityComponentManager_TEST.cc EventManager_TEST.cc - ign_TEST.cc Link_TEST.cc Model_TEST.cc - ModelCommandAPI_TEST.cc + Primitives_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc - Server_TEST.cc ServerConfig_TEST.cc + Server_TEST.cc SimulationRunner_TEST.cc - System_TEST.cc SystemLoader_TEST.cc + System_TEST.cc TestFixture_TEST.cc Util_TEST.cc World_TEST.cc + ign_TEST.cc network/NetworkConfig_TEST.cc network/PeerTracker_TEST.cc network/NetworkManager_TEST.cc ) +# Tests that require a valid display +set(tests_needing_display + ModelCommandAPI_TEST.cc +) + +# Add systems that need a valid display here. +# \todo(anyone) Find a way to run these tests with a virtual display such Xvfb +# or Xdummy instead of skipping them +if(VALID_DISPLAY AND VALID_DRI_DISPLAY) + list(APPEND gtest_sources ${tests_needing_display}) +else() + message(STATUS + "Skipping these UNIT tests because a valid display was not found:") + foreach(test ${tests_needing_display}) + message(STATUS " ${test}") + endforeach(test) +endif() + if (MSVC) # Warning #4251 is the "dll-interface" warning that tells you when types used # by a class are not being exported. These generated source files have private diff --git a/src/ComponentFactory_TEST.cc b/src/ComponentFactory_TEST.cc index 0503f2c07e3..e9adeb972a5 100644 --- a/src/ComponentFactory_TEST.cc +++ b/src/ComponentFactory_TEST.cc @@ -19,6 +19,7 @@ #include "ignition/gazebo/test_config.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/Pose.hh" #include "../test/helpers/EnvTestFixture.hh" @@ -54,8 +55,7 @@ TEST_F(ComponentFactoryTest, Register) auto registeredCount = factory->TypeIds().size(); factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); // Check now it has type id EXPECT_NE(0u, MyCustom::typeId); @@ -70,8 +70,7 @@ TEST_F(ComponentFactoryTest, Register) // Fail to register same component twice factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EXPECT_EQ(registeredCount + 1, factory->TypeIds().size()); @@ -79,8 +78,7 @@ TEST_F(ComponentFactoryTest, Register) using Duplicate = components::Component; factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EXPECT_EQ(registeredCount + 1, factory->TypeIds().size()); @@ -101,12 +99,12 @@ TEST_F(ComponentFactoryTest, New) { auto comp = factory->New(123456789); - ASSERT_TRUE(comp == nullptr); + ASSERT_EQ(nullptr, comp); } { auto comp = factory->New(); - ASSERT_TRUE(comp != nullptr); + ASSERT_NE(nullptr, comp); EXPECT_NE(0u, comp->typeId); EXPECT_EQ(comp->typeId, components::Pose::typeId); @@ -114,19 +112,33 @@ TEST_F(ComponentFactoryTest, New) { auto comp = factory->New(components::Pose::typeId); - ASSERT_TRUE(comp != nullptr); + ASSERT_NE(nullptr, comp); EXPECT_NE(0u, comp->TypeId()); - EXPECT_TRUE(nullptr != static_cast(comp.get())); + EXPECT_NE(nullptr, static_cast(comp.get())); } { - auto storage = factory->NewStorage(components::Pose::typeId); - ASSERT_TRUE(storage != nullptr); + // Test constructing a component with pre-defined data - EXPECT_NE(nullptr, static_cast *>( - storage.get())); + // Test a valid pre-defined component + ignition::math::Pose3d pose(1, 2, 3, 4, 5, 6); + components::Pose poseComp(pose); + auto comp = factory->New(components::Pose::typeId, &poseComp); + ASSERT_NE(nullptr, comp); + EXPECT_NE(0u, comp->TypeId()); + auto derivedComp = static_cast(comp.get()); + ASSERT_NE(nullptr, derivedComp); + EXPECT_EQ(pose, derivedComp->Data()); + + // Test an invalid pre-defined component + comp = factory->New(components::Pose::typeId, nullptr); + ASSERT_EQ(nullptr, comp); + + // Test mistmatching component types + comp = factory->New(components::Name::typeId, &poseComp); + ASSERT_EQ(nullptr, comp); } } diff --git a/src/Component_TEST.cc b/src/Component_TEST.cc index 6618a8edc9b..2202b057535 100644 --- a/src/Component_TEST.cc +++ b/src/Component_TEST.cc @@ -69,8 +69,7 @@ TEST_F(ComponentTest, DataByMove) using CustomComponent = components::Component, class CustomComponentTag>; factory->Register("ign_gazebo_components.MyCustom", - new components::ComponentDescriptor(), - new components::StorageDescriptor()); + new components::ComponentDescriptor()); EntityComponentManager ecm; Entity entity = ecm.CreateEntity(); @@ -178,6 +177,11 @@ class NoSerialize : public components::BaseComponent { return 0; } + + public: std::unique_ptr Clone() override + { + return nullptr; + } }; ////////////////////////////////////////////////// @@ -551,3 +555,40 @@ TEST_F(ComponentTest, TypeName) EXPECT_EQ("123456", comp.typeName); } } + +////////////////////////////////////////////////// +TEST_F(ComponentTest, Clone) +{ + // Component with data + { + using Custom = components::Component; + + // create a component and a clone of it. The clone should initially have the + // same data as the original component + Custom comp(5); + auto clonedComp = comp.Clone(); + auto derivedClone = static_cast(clonedComp.get()); + EXPECT_EQ(comp, *derivedClone); + + // modify the data of the cloned component, and make sure that only the + // cloned component is modified, not the original component + derivedClone->Data() = 10; + EXPECT_NE(comp, *derivedClone); + EXPECT_EQ(5, comp.Data()); + EXPECT_EQ(10, derivedClone->Data()); + } + + // Component without data + { + using Custom = components::Component; + + Custom comp; + auto clonedComp = comp.Clone(); + auto derivedClone = static_cast(clonedComp.get()); + + // since this component has no data, we cannot do the same check as we did + // above for a component with data. However, we can make sure that the + // pointers for the components are different + EXPECT_NE(&comp, derivedClone); + } +} diff --git a/src/Conversions.cc b/src/Conversions.cc index a71ef0aafb9..d7d308d27bb 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -1560,13 +1560,7 @@ msgs::ParticleEmitter ignition::gazebo::convert(const sdf::ParticleEmitter &_in) header->add_value(_in.Topic()); } - // todo(anyone) Use particle_scatter_ratio in particle_emitter.proto from - // Fortress on. - auto header = out.mutable_header()->add_data(); - header->set_key("particle_scatter_ratio"); - std::string *value = header->add_value(); - *value = std::to_string(_in.ScatterRatio()); - + out.mutable_particle_scatter_ratio()->set_data(_in.ScatterRatio()); return out; } @@ -1618,6 +1612,8 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) out.SetScaleRate(_in.scale_rate().data()); if (_in.has_color_range_image()) out.SetColorRangeImage(_in.color_range_image().data()); + if (_in.has_particle_scatter_ratio()) + out.SetScatterRatio(_in.particle_scatter_ratio().data()); for (int i = 0; i < _in.header().data_size(); ++i) { @@ -1626,10 +1622,6 @@ sdf::ParticleEmitter ignition::gazebo::convert(const msgs::ParticleEmitter &_in) { out.SetTopic(data.value(0)); } - else if (data.key() == "particle_scatter_ratio" && data.value_size() > 0) - { - out.SetScatterRatio(std::stof(data.value(0))); - } } return out; diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index ce84b9bb98b..bfc7f691cf9 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -997,9 +997,7 @@ TEST(Conversions, ParticleEmitter) EXPECT_EQ("topic", header.key()); EXPECT_EQ("my_topic", header.value(0)); - auto headerScatterRatio = emitterMsg.header().data(1); - EXPECT_EQ("particle_scatter_ratio", headerScatterRatio.key()); - EXPECT_FLOAT_EQ(0.9f, std::stof(headerScatterRatio.value(0))); + EXPECT_FLOAT_EQ(0.9f, emitterMsg.particle_scatter_ratio().data()); EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), msgs::Convert(emitterMsg.pose())); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 5f4c4fe0a9a..f0d1b4d4d7f 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -15,17 +15,30 @@ * */ +#include "ignition/gazebo/EntityComponentManager.hh" + #include +#include #include +#include +#include #include #include +#include #include #include #include + +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Component.hh" #include "ignition/gazebo/components/Factory.hh" -#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" using namespace ignition; using namespace gazebo; @@ -52,11 +65,6 @@ class ignition::gazebo::EntityComponentManagerPrivate public: void EraseEntityRecursive(Entity _entity, std::unordered_set &_set); - /// \brief Register a new component type. - /// \param[in] _typeId Type if of the new component. - /// \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(); @@ -85,20 +93,49 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \param[in] _entity Entity that has component newly modified public: void AddModifiedComponent(const Entity &_entity); - /// \brief Map of component storage classes. The key is a component - /// type id, and the value is a pointer to the component storage. - public: std::unordered_map> components; + /// \brief Check whether a component is marked as a component that is + /// currently removed or not. + /// \param[in] _entity The entity + /// \param[in] _typeId The type ID for the component that belongs to _entity + /// \return True if _entity has a component of type _typeId that is currently + /// removed. False otherwise + public: bool ComponentMarkedAsRemoved(const Entity _entity, + const ComponentTypeId _typeId) const; + + /// \brief Set a cloned joint's parent or child link name. + /// \param[in] _joint The cloned joint. + /// \param[in] _originalLink The original joint's parent or child link. + /// \param[in] _ecm Entity component manager. + /// \tparam The component type, which must be either + /// components::ParentLinkName or components::ChildLinkName + /// \return True if _joint's parent or child link name was set. + /// False otherwise + /// \note This method should only be called in EntityComponentManager::Clone. + /// This is a temporary workaround until we find a way to clone entites and + /// components that don't require special treatment for particular component + /// types. + public: template + bool ClonedJointLinkName(Entity _joint, Entity _originalLink, + EntityComponentManager *_ecm); + + /// \brief All component types that have ever been created. + public: std::unordered_set createdCompTypes; /// \brief A graph holding all entities, arranged according to their /// parenting. public: EntityGraph entities; - /// \brief Components that have been changed through a peridic change. - public: std::set periodicChangedComponents; + /// \brief Components that have been changed through a periodic change. + /// The key is the type of component which has changed, and the value is the + /// entities that had this type of component changed. + public: std::unordered_map> + periodicChangedComponents; /// \brief Components that have been changed through a one-time change. - public: std::set oneTimeChangedComponents; + /// The key is the type of component which has changed, and the value is the + /// entities that had this type of component changed. + public: std::unordered_map> + oneTimeChangedComponents; /// \brief Entities that have just been created public: std::unordered_set newlyCreatedEntities; @@ -115,26 +152,6 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Flag that indicates if all entities should be removed. public: bool removeAllEntities{false}; - /// \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; @@ -148,7 +165,15 @@ class ignition::gazebo::EntityComponentManagerPrivate public: mutable std::mutex removedComponentsMutex; /// \brief The set of all views. - public: mutable std::map views; + /// The value is a pair of the view itself and a mutex that can be used for + /// locking the view to ensure thread safety when adding entities to the view. + public: mutable std::unordered_map, + std::unique_ptr>, detail::ComponentTypeHasher> views; + + /// \brief A flag that indicates whether views should be locked while adding + /// new entities to them or not. + public: bool lockAddEntitiesToViews{false}; /// \brief Cache of previously queried descendants. The key is the parent /// entity for which descendants were queried, and the value are all its @@ -159,10 +184,93 @@ class ignition::gazebo::EntityComponentManagerPrivate /// \brief Keep track of entities already used to ensure uniqueness. public: uint64_t entityCount{0}; - /// \brief Unordered multimap of removed components. The key is the entity to - /// which belongs the component, and the value is the component being - /// removed. - std::unordered_multimap removedComponents; + /// \brief Unordered map of removed components. The key is the entity to + /// which belongs the component, and the value is a set of the component types + /// being removed. + public: std::unordered_map> + removedComponents; + + /// \brief All components that have been removed. The difference between + /// removedComponents and componentsMarkedAsRemoved is that removedComponents + /// keeps track of components that were removed in the current simulation + /// step, while componentsMarkedAsRemoved keeps track of components that are + /// currently removed based on all simulation steps. + public: std::unordered_map> + componentsMarkedAsRemoved; + + /// \brief A map of an entity to its components + public: std::unordered_map>> + componentStorage; + + /// \brief A map that keeps track of where each type of component is + /// located in the componentStorage vector. Since the componentStorage vector + /// is of type BaseComponent, we need to keep track of which component type + /// corresponds to a given index in the vector so that we can cast the + /// BaseComponent to this type if needed. + /// + /// The key of this map is the Entity, and the value is a map of the + /// component type to the corresponding index in the + /// componentStorage vector (a component of a particular type is + /// only a key for the value map if a component of this type exists in + /// the componentStorage vector) + /// + /// NOTE: Any modification of this data structure must be followed + /// by setting `componentTypeIndexDirty` to true. + public: std::unordered_map> + componentTypeIndex; + + /// \brief A vector of iterators to evenly distributed spots in the + /// `componentTypeIndex` map. Threads in the `State` function use this + /// vector for easy access of their pre-allocated work. This vector + /// is recalculated if `componentTypeIndex` is changed (when + /// `componentTypeIndexDirty` == true). + public: std::vector>::iterator> + componentTypeIndexIterators; + + /// \brief True if the componentTypeIndex map was changed. Primarily used + /// by the multithreading functionality in `State()` to allocate work to + /// each thread. + public: bool componentTypeIndexDirty{true}; + + /// \brief During cloning, we populate two maps: + /// - map of cloned model entities to the non-cloned model's canonical link + /// - map of non-cloned canonical links to the cloned canonical link + /// After cloning is done, these maps can be used to update the cloned model's + /// canonical link to be the cloned canonical link instead of the original + /// model's canonical link. We populate maps during cloning and then update + /// canonical links after cloning since cloning is done top-down, and + /// canonical links are children of models (when a model is cloned, its + /// canonical link has not been cloned yet, so we have no way of knowing what + /// to set the cloned model's canonical link to until the canonical link has + /// been cloned). + /// \TODO(anyone) We shouldn't be giving canonical links special treatment. + /// This may happen to any component that holds an Entity, so we should figure + /// out a way to generalize this for any such component. + public: std::unordered_map oldModelCanonicalLink; + + /// \brief See above + public: std::unordered_map oldToClonedCanonicalLink; + + /// \brief During cloning, we populate two maps: + /// - map of link entities to their cloned link + /// - map of cloned joint entities to the original joint entity's parent and + /// child links + /// After cloning is done, these maps can be used to update the cloned joint + /// entity's parent and child links to the cloned parent and child links. + /// \TODO(anyone) We shouldn't be giving joints special treatment. + /// We should figure out a way to update a joint's parent/child links without + /// having to explicitly search/track for the cloned links. + public: std::unordered_map originalToClonedLink; + + /// \brief See above + /// The key is the cloned joint entity, and the value is a pair where the + /// first element is the original joint's parent link, and the second element + /// is the original joint's child link + public: std::unordered_map> + clonedToOriginalJointLinks; /// \brief Set of entities that are prevented from removal. public: std::unordered_set pinnedEntities; @@ -213,9 +321,230 @@ Entity EntityComponentManagerPrivate::CreateEntityImplementation(Entity _entity) // Reset descendants cache this->descendantCache.clear(); + const auto result = this->componentStorage.insert({_entity, + std::vector>()}); + if (!result.second) + { + ignwarn << "Attempted to add entity [" << _entity + << "] to component storage, but this entity is already in component " + << "storage.\n"; + } + + const auto result2 = this->componentTypeIndex.insert({_entity, + std::unordered_map()}); + if (!result2.second) + { + ignwarn << "Attempted to add entity [" << _entity + << "] to component type index, but this entity is already in component " + << "type index.\n"; + } + return _entity; } +///////////////////////////////////////////////// +Entity EntityComponentManager::Clone(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename) +{ + // Clear maps so they're populated for the entity being cloned + this->dataPtr->oldToClonedCanonicalLink.clear(); + this->dataPtr->oldModelCanonicalLink.clear(); + this->dataPtr->originalToClonedLink.clear(); + this->dataPtr->clonedToOriginalJointLinks.clear(); + + auto clonedEntity = this->CloneImpl(_entity, _parent, _name, _allowRename); + + if (kNullEntity != clonedEntity) + { + // make sure that cloned models have their canonical link updated to the + // cloned canonical link + for (const auto &[clonedModel, oldCanonicalLink] : + this->dataPtr->oldModelCanonicalLink) + { + auto iter = this->dataPtr->oldToClonedCanonicalLink.find( + oldCanonicalLink); + if (iter == this->dataPtr->oldToClonedCanonicalLink.end()) + { + ignerr << "Error: attempted to clone model(s) with canonical link(s), " + << "but entity [" << oldCanonicalLink << "] was not cloned as a " + << "canonical link." << std::endl; + continue; + } + const auto clonedCanonicalLink = iter->second; + this->SetComponentData(clonedModel, + clonedCanonicalLink); + } + + // make sure that cloned joints have their parent/child links + // updated to the cloned parent/child links + for (const auto &[clonedJoint, originalJointLinks] : + this->dataPtr->clonedToOriginalJointLinks) + { + auto originalParentLink = originalJointLinks.first; + if (!this->dataPtr->ClonedJointLinkName( + clonedJoint, originalParentLink, this)) + { + ignerr << "Error updating the cloned parent link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + + auto originalChildLink = originalJointLinks.second; + if (!this->dataPtr->ClonedJointLinkName( + clonedJoint, originalChildLink, this)) + { + ignerr << "Error updating the cloned child link name for cloned " + << "joint [" << clonedJoint << "]\n"; + continue; + } + } + } + + return clonedEntity; +} + +///////////////////////////////////////////////// +Entity EntityComponentManager::CloneImpl(Entity _entity, Entity _parent, + const std::string &_name, bool _allowRename) +{ + auto uniqueNameGenerated = false; + + // Before cloning, we should make sure that: + // 1. The entity to be cloned exists + // 2. We can generate a unique name for the cloned entity + if (!this->HasEntity(_entity)) + { + ignerr << "Requested to clone entity [" << _entity + << "], but this entity does not exist." << std::endl; + return kNullEntity; + } + else if (!_name.empty() && !_allowRename) + { + if (kNullEntity != this->EntityByComponents(components::Name(_name))) + { + ignerr << "Requested to clone entity [" << _entity + << "] with a name of [" << _name << "], but another entity already " + << "has this name." << std::endl; + return kNullEntity; + } + uniqueNameGenerated = true; + } + + auto clonedEntity = this->CreateEntity(); + + if (_parent != kNullEntity) + { + this->SetParentEntity(clonedEntity, _parent); + this->CreateComponent(clonedEntity, components::ParentEntity(_parent)); + } + + // make sure that the cloned entity has a unique name + auto clonedName = _name; + if (!uniqueNameGenerated) + { + if (clonedName.empty()) + { + auto originalNameComp = this->Component(_entity); + clonedName = + originalNameComp ? originalNameComp->Data() : "cloned_entity"; + } + uint64_t suffix = 1; + while (kNullEntity != this->EntityByComponents( + components::Name(clonedName + "_" + std::to_string(suffix)))) + suffix++; + clonedName += "_" + std::to_string(suffix); + } + this->CreateComponent(clonedEntity, components::Name(clonedName)); + + // copy all components from _entity to clonedEntity + for (const auto &type : this->ComponentTypes(_entity)) + { + // skip the Name and ParentEntity components since those were already + // handled above + if ((type == components::Name::typeId) || + (type == components::ParentEntity::typeId)) + continue; + + auto originalComp = this->ComponentImplementation(_entity, type); + auto clonedComp = originalComp->Clone(); + + this->CreateComponentImplementation(clonedEntity, type, clonedComp.get()); + } + + // keep track of canonical link information (for clones of models, the cloned + // model should not share the same canonical link as the original model) + if (auto modelCanonLinkComp = + this->Component(clonedEntity)) + { + // we're cloning a model, so we map the cloned model to the original + // model's canonical link + this->dataPtr->oldModelCanonicalLink[clonedEntity] = + modelCanonLinkComp->Data(); + } + else if (this->Component(clonedEntity)) + { + // we're cloning a canonical link, so we map the original canonical link + // to the cloned canonical link + this->dataPtr->oldToClonedCanonicalLink[_entity] = clonedEntity; + } + + // keep track of all joints and links that have been cloned so that cloned + // joints can be updated to their cloned parent/child links + if (this->Component(clonedEntity)) + { + // this is a joint, so we need to find the original joint's parent and child + // link entities + Entity originalParentLink = kNullEntity; + Entity originalChildLink = kNullEntity; + + const auto &parentName = + this->Component(_entity); + if (parentName) + { + originalParentLink = this->EntityByComponents( + components::Name(parentName->Data())); + } + + const auto &childName = this->Component(_entity); + if (childName) + { + originalChildLink = this->EntityByComponents( + components::Name(childName->Data())); + } + + if (!originalParentLink || !originalChildLink) + { + ignerr << "The cloned joint entity [" << clonedEntity << "] was unable " + << "to find the original joint entity's parent and/or child link.\n"; + this->RequestRemoveEntity(clonedEntity); + return kNullEntity; + } + + this->dataPtr->clonedToOriginalJointLinks[clonedEntity] = + {originalParentLink, originalChildLink}; + } + else if (this->Component(clonedEntity) || + this->Component(clonedEntity)) + { + // save a mapping between the original link and the cloned link + this->dataPtr->originalToClonedLink[_entity] = clonedEntity; + } + + for (const auto &childEntity : + this->EntitiesByComponents(components::ParentEntity(_entity))) + { + auto clonedChild = this->CloneImpl(childEntity, clonedEntity, "", true); + if (kNullEntity == clonedChild) + { + ignerr << "Cloning child entity [" << childEntity << "] failed.\n"; + this->RequestRemoveEntity(clonedEntity); + return kNullEntity; + } + } + + return clonedEntity; +} + ///////////////////////////////////////////////// void EntityComponentManager::ClearNewlyCreatedEntities() { @@ -224,7 +553,7 @@ void EntityComponentManager::ClearNewlyCreatedEntities() for (auto &view : this->dataPtr->views) { - view.second.ClearNewEntities(); + view.second.first->ResetNewEntityState(); } } @@ -298,7 +627,10 @@ void EntityComponentManager::RequestRemoveEntity(Entity _entity, for (const auto &removedEntity : tmpToRemoveEntities) { - this->UpdateViews(removedEntity); + for (auto &view : this->dataPtr->views) + { + view.second.first->MarkEntityToRemove(removedEntity); + } } } @@ -317,8 +649,8 @@ void EntityComponentManager::RequestRemoveEntities() { std::unordered_set tmpToRemoveEntities; - // Store the to-be-removed entities in a temporary set so we can call - // UpdateViews on each of them + // Store the to-be-removed entities in a temporary set so we can + // mark each of them to be removed from views that contain them. for (const auto &vertex : this->dataPtr->entities.Vertices()) { if (std::find(this->dataPtr->pinnedEntities.begin(), @@ -337,7 +669,10 @@ void EntityComponentManager::RequestRemoveEntities() for (const auto &removedEntity : tmpToRemoveEntities) { - this->UpdateViews(removedEntity); + for (auto &view : this->dataPtr->views) + { + view.second.first->MarkEntityToRemove(removedEntity); + } } } } @@ -353,15 +688,13 @@ void EntityComponentManager::ProcessRemoveEntityRequests() IGN_PROFILE("RemoveAll"); this->dataPtr->removeAllEntities = false; this->dataPtr->entities = EntityGraph(); - this->dataPtr->entityComponents.clear(); this->dataPtr->toRemoveEntities.clear(); - this->dataPtr->entityComponentsDirty = true; + this->dataPtr->componentsMarkedAsRemoved.clear(); - for (std::pair> &comp: this->dataPtr->components) - { - comp.second->RemoveAll(); - } + // reset the entity component storage + this->dataPtr->componentStorage.clear(); + this->dataPtr->componentTypeIndex.clear(); + this->dataPtr->componentTypeIndexDirty = true; // All views are now invalid. this->dataPtr->views.clear(); @@ -379,24 +712,15 @@ void EntityComponentManager::ProcessRemoveEntityRequests() // Remove from graph this->dataPtr->entities.RemoveVertex(entity); - auto entityIter = this->dataPtr->entityComponents.find(entity); - // Remove the components, if any. - if (entityIter != this->dataPtr->entityComponents.end()) - { - for (const auto &key : entityIter->second) - { - this->dataPtr->components.at(key.first)->Remove(key.second); - } - - // Remove the entry in the entityComponent map - this->dataPtr->entityComponents.erase(entity); - this->dataPtr->entityComponentsDirty = true; - } + this->dataPtr->componentsMarkedAsRemoved.erase(entity); + this->dataPtr->componentStorage.erase(entity); + this->dataPtr->componentTypeIndex.erase(entity); + this->dataPtr->componentTypeIndexDirty = true; // Remove the entity from views. for (auto &view : this->dataPtr->views) { - view.second.RemoveEntity(entity, view.first); + view.second.first->RemoveEntity(entity); } } // Clear the set of entities to remove. @@ -410,48 +734,61 @@ void EntityComponentManager::ProcessRemoveEntityRequests() ///////////////////////////////////////////////// bool EntityComponentManager::RemoveComponent( const Entity _entity, const ComponentTypeId &_typeId) -{ - auto componentId = this->EntityComponentIdFromType(_entity, _typeId); - ComponentKey key{_typeId, componentId}; - return this->RemoveComponent(_entity, key); -} - -///////////////////////////////////////////////// -bool EntityComponentManager::RemoveComponent( - const Entity _entity, const ComponentKey &_key) { IGN_PROFILE("EntityComponentManager::RemoveComponent"); // Make sure the entity exists and has the component. - if (!this->EntityHasComponent(_entity, _key)) + if (!this->EntityHasComponentType(_entity, _typeId)) return false; - this->dataPtr->components.at(_key.first)->Remove(_key.second); - this->dataPtr->entityComponents[_entity].erase(_key.first); - this->dataPtr->oneTimeChangedComponents.erase(_key); - this->dataPtr->periodicChangedComponents.erase(_key); - this->dataPtr->entityComponentsDirty = true; + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_typeId); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + { + oneTimeIter->second.erase(_entity); + if (oneTimeIter->second.empty()) + this->dataPtr->oneTimeChangedComponents.erase(oneTimeIter); + } - this->UpdateViews(_entity); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_typeId); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + { + periodicIter->second.erase(_entity); + if (periodicIter->second.empty()) + this->dataPtr->periodicChangedComponents.erase(periodicIter); + } + + auto compPtr = this->ComponentImplementation(_entity, _typeId); + if (compPtr) + { + this->dataPtr->componentsMarkedAsRemoved[_entity].insert(_typeId); + + // update views to reflect the component removal + for (auto &viewPair : this->dataPtr->views) + viewPair.second.first->NotifyComponentRemoval(_entity, _typeId); + } this->dataPtr->AddModifiedComponent(_entity); // Add component to map of removed components { std::lock_guard lock(this->dataPtr->removedComponentsMutex); - this->dataPtr->removedComponents.insert(std::make_pair(_entity, _key)); + this->dataPtr->removedComponents[_entity].insert(_typeId); } return true; } +///////////////////////////////////////////////// +bool EntityComponentManager::RemoveComponent( + const Entity _entity, const ComponentKey &_key) +{ + return this->RemoveComponent(_entity, _key.first); +} + ///////////////////////////////////////////////// bool EntityComponentManager::EntityHasComponent(const Entity _entity, const ComponentKey &_key) const { - if (!this->HasEntity(_entity)) - return false; - auto &compMap = this->dataPtr->entityComponents[_entity]; - return compMap.find(_key.first) != compMap.end(); + return this->EntityHasComponentType(_entity, _key.first); } ///////////////////////////////////////////////// @@ -461,13 +798,9 @@ bool EntityComponentManager::EntityHasComponentType(const Entity _entity, if (!this->HasEntity(_entity)) return false; - auto iter = this->dataPtr->entityComponents.find(_entity); - - if (iter == this->dataPtr->entityComponents.end()) - return false; + auto comp = this->ComponentImplementation(_entity, _typeId); - auto typeIter = iter->second.find(_typeId); - return (typeIter != iter->second.end()); + return comp != nullptr; } ///////////////////////////////////////////////// @@ -496,26 +829,31 @@ ComponentState EntityComponentManager::ComponentState(const Entity _entity, { auto result = ComponentState::NoChange; - auto ecIter = this->dataPtr->entityComponents.find(_entity); + auto ctIter = this->dataPtr->componentTypeIndex.find(_entity); - if (ecIter == this->dataPtr->entityComponents.end()) + if (ctIter == this->dataPtr->componentTypeIndex.end()) return result; - auto typeKey = ecIter->second.find(_typeId); - if (typeKey == ecIter->second.end()) + auto typeIter = ctIter->second.find(_typeId); + if (typeIter == ctIter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, _typeId)) return result; - ComponentKey key{_typeId, typeKey->second}; + auto typeId = typeIter->first; - if (this->dataPtr->oneTimeChangedComponents.find(key) != - this->dataPtr->oneTimeChangedComponents.end()) + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(typeId); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end() && + oneTimeIter->second.find(_entity) != oneTimeIter->second.end()) { result = ComponentState::OneTimeChange; } - else if (this->dataPtr->periodicChangedComponents.find(key) != - this->dataPtr->periodicChangedComponents.end()) + else { - result = ComponentState::PeriodicChange; + auto periodicIter = + this->dataPtr->periodicChangedComponents.find(typeId); + if (periodicIter != this->dataPtr->periodicChangedComponents.end() && + periodicIter->second.find(_entity) != periodicIter->second.end()) + result = ComponentState::PeriodicChange; } return result; @@ -547,9 +885,9 @@ std::unordered_set EntityComponentManager::ComponentTypesWithPeriodicChanges() const { std::unordered_set periodicComponents; - for (const auto& compPair : this->dataPtr->periodicChangedComponents) + for (const auto& typeToEntityPtrs : this->dataPtr->periodicChangedComponents) { - periodicComponents.insert(compPair.first); + periodicComponents.insert(typeToEntityPtrs.first); } return periodicComponents; } @@ -596,7 +934,7 @@ bool EntityComponentManager::SetParentEntity(const Entity _child, } ///////////////////////////////////////////////// -ComponentKey EntityComponentManager::CreateComponentImplementation( +bool EntityComponentManager::CreateComponentImplementation( const Entity _entity, const ComponentTypeId _componentTypeId, const components::BaseComponent *_data) { @@ -606,48 +944,114 @@ ComponentKey EntityComponentManager::CreateComponentImplementation( ignerr << "Trying to create a component of type [" << _componentTypeId << "] attached to entity [" << _entity << "], but this entity does not " << "exist. This create component request will be ignored." << std::endl; - return ComponentKey(); + return false; } - // If type hasn't been instantiated yet, create a storage for it - if (!this->HasComponentType(_componentTypeId)) + // if this is the first time this component type is being created, make sure + // the component type to be created is valid + if (!this->HasComponentType(_componentTypeId) && + !components::Factory::Instance()->HasType(_componentTypeId)) { - if (!this->dataPtr->CreateComponentStorage(_componentTypeId)) - { - ignerr << "Failed to create component of type [" << _componentTypeId - << "] for entity [" << _entity - << "]. Type has not been properly registered." << std::endl; - return ComponentKey(); - } + ignerr << "Failed to create component of type [" << _componentTypeId + << "] for entity [" << _entity + << "]. Type has not been properly registered." << std::endl; + return false; } + // assume the component data needs to be updated externally unless this + // component is a brand new creation/addition + bool updateData = true; + this->dataPtr->AddModifiedComponent(_entity); + this->dataPtr->oneTimeChangedComponents[_componentTypeId].insert(_entity); - // Instantiate the new component. - std::pair componentIdPair = - this->dataPtr->components[_componentTypeId]->Create(_data); + // make sure the entity exists + auto typeMapIter = this->dataPtr->componentTypeIndex.find(_entity); + if (typeMapIter == this->dataPtr->componentTypeIndex.end()) + { + ignerr << "Attempt to create a component of type [" << _componentTypeId + << "] attached to entity [" << _entity + << "] failed: entity not in componentTypeIndex." << std::endl; + return false; + } + + auto entityCompIter = this->dataPtr->componentStorage.find(_entity); + if (entityCompIter == this->dataPtr->componentStorage.end()) + { + ignerr << "Attempt to create a component of type [" << _componentTypeId + << "] attached to entity [" << _entity + << "] failed: entity not in storage." << std::endl; + return false; + } - ComponentKey componentKey{_componentTypeId, componentIdPair.first}; + // Instantiate the new component. + auto newComp = components::Factory::Instance()->New(_componentTypeId, _data); - this->dataPtr->entityComponents[_entity].insert( - {_componentTypeId, componentIdPair.first}); - this->dataPtr->oneTimeChangedComponents.insert(componentKey); - this->dataPtr->entityComponentsDirty = true; + const auto compIdxIter = typeMapIter->second.find(_componentTypeId); + // If entity has never had a component of this type + if (compIdxIter == typeMapIter->second.end()) + { + const auto vectorIdx = entityCompIter->second.size(); + entityCompIter->second.push_back(std::move(newComp)); + this->dataPtr->componentTypeIndex[_entity][_componentTypeId] = vectorIdx; + this->dataPtr->componentTypeIndexDirty = true; - if (componentIdPair.second) - this->RebuildViews(); + updateData = false; + for (auto &viewPair : this->dataPtr->views) + { + auto &view = viewPair.second.first; + if (this->EntityMatches(_entity, view->ComponentTypes())) + view->MarkEntityToAdd(_entity, this->IsNewEntity(_entity)); + } + } else - this->UpdateViews(_entity); + { + // if the pre-existing component is marked as removed, this means that the + // component was added to the entity previously, but later removed. In this + // case, a re-addition of the component is occuring. If the pre-existing + // component is not marked as removed, this means that the component was + // added to the entity previously and never removed. In this case, we are + // simply modifying the data of the pre-existing component (the modification + // of the data is done externally in a templated ECM method call, because we + // need the derived component class in order to update the derived component + // data) + auto existingCompPtr = entityCompIter->second.at(compIdxIter->second).get(); + if (!existingCompPtr) + { + ignerr << "Internal error: entity [" << _entity << "] has a component of " + << "type [" << _componentTypeId << "] in the storage, but the instance " + << "of this component is nullptr. This should never happen!" + << std::endl; + return false; + } + else if (this->dataPtr->ComponentMarkedAsRemoved(_entity, _componentTypeId)) + { + this->dataPtr->componentsMarkedAsRemoved[_entity].erase(_componentTypeId); + + for (auto &viewPair : this->dataPtr->views) + { + viewPair.second.first->NotifyComponentAddition(_entity, + this->IsNewEntity(_entity), _componentTypeId); + } + } + } - return componentKey; + this->dataPtr->createdCompTypes.insert(_componentTypeId); + + return updateData; } ///////////////////////////////////////////////// bool EntityComponentManager::EntityMatches(Entity _entity, const std::set &_types) const { - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) + return false; + + // quick check: the entity cannot match _types if _types is larger than the + // number of component types the entity has + if (_types.size() > iter->second.size()) return false; // \todo(nkoenig) The performance of this could be improved. @@ -657,127 +1061,74 @@ bool EntityComponentManager::EntityMatches(Entity _entity, for (const ComponentTypeId &type : _types) { auto typeIter = iter->second.find(type); - if (typeIter == iter->second.end()) + if (typeIter == iter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, type)) return false; } return true; } -///////////////////////////////////////////////// -ComponentId EntityComponentManager::EntityComponentIdFromType( - const Entity _entity, const ComponentTypeId _type) const -{ - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) - return -1; - - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return typeIter->second; - - return -1; -} - ///////////////////////////////////////////////// const components::BaseComponent *EntityComponentManager::ComponentImplementation( const Entity _entity, const ComponentTypeId _type) const { IGN_PROFILE("EntityComponentManager::ComponentImplementation"); - auto ecIter = this->dataPtr->entityComponents.find(_entity); - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure the entity exists + const auto typeMapIter = this->dataPtr->componentTypeIndex.find(_entity); + if (typeMapIter == this->dataPtr->componentTypeIndex.end()) return nullptr; - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(_type)->Component( - typeIter->second); - - return nullptr; -} - -///////////////////////////////////////////////// -components::BaseComponent *EntityComponentManager::ComponentImplementation( - const Entity _entity, const ComponentTypeId _type) -{ - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure the component type exists for the entity + const auto compIdxIter = typeMapIter->second.find(_type); + if (compIdxIter == typeMapIter->second.end()) return nullptr; - auto typeIter = ecIter->second.find(_type); - if (typeIter != ecIter->second.end()) - return this->dataPtr->components.at(_type)->Component(typeIter->second); - - return nullptr; -} + // get the pointer to the component + const auto compVecIter = this->dataPtr->componentStorage.find(_entity); + if (compVecIter == this->dataPtr->componentStorage.end()) + { + ignerr << "Internal error: Entity [" << _entity + << "] is missing in storage, but is in " + << "componentTypeIndex. This should never happen!" << std::endl; + return nullptr; + } -///////////////////////////////////////////////// -const components::BaseComponent - *EntityComponentManager::ComponentImplementation( - const ComponentKey &_key) const -{ - if (this->dataPtr->components.find(_key.first) != - this->dataPtr->components.end()) + auto compPtr = compVecIter->second.at(compIdxIter->second).get(); + if (nullptr == compPtr) { - return this->dataPtr->components.at(_key.first)->Component(_key.second); + ignerr << "Internal error: entity [" << _entity << "] has a component of " + << "type [" << _type << "] in the storage, but the instance " + << "of this component is nullptr. This should never happen!" + << std::endl; + return nullptr; } + + // Return component if not marked as removed. + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, _type)) + return compPtr; + return nullptr; } ///////////////////////////////////////////////// components::BaseComponent *EntityComponentManager::ComponentImplementation( - const ComponentKey &_key) + const Entity _entity, const ComponentTypeId _type) { - if (this->dataPtr->components.find(_key.first) != - this->dataPtr->components.end()) - { - return this->dataPtr->components.at(_key.first)->Component(_key.second); - } - return nullptr; + // Call the const version of the function + return const_cast( + static_cast( + *this).ComponentImplementation(_entity, _type)); } ///////////////////////////////////////////////// bool EntityComponentManager::HasComponentType( const ComponentTypeId _typeId) const { - return this->dataPtr->components.find(_typeId) != - this->dataPtr->components.end(); -} - -///////////////////////////////////////////////// -bool EntityComponentManagerPrivate::CreateComponentStorage( - const ComponentTypeId _typeId) -{ - auto storage = components::Factory::Instance()->NewStorage(_typeId); - - if (nullptr == storage) - { - ignerr << "Internal errror: failed to create storage for type [" << _typeId - << "]" << std::endl; - return false; - } - - this->components[_typeId] = std::move(storage); - igndbg << "Using components of type [" << _typeId << "] / [" - << components::Factory::Instance()->Name(_typeId) << "].\n"; - - return true; -} - -///////////////////////////////////////////////// -components::BaseComponent *EntityComponentManager::First( - const ComponentTypeId _componentTypeId) -{ - auto iter = this->dataPtr->components.find(_componentTypeId); - if (iter != this->dataPtr->components.end()) - { - return iter->second->First(); - } - return nullptr; + return this->dataPtr->createdCompTypes.find(_typeId) != + this->dataPtr->createdCompTypes.end(); } ////////////////////////////////////////////////// @@ -787,85 +1138,56 @@ const EntityGraph &EntityComponentManager::Entities() const } ////////////////////////////////////////////////// -bool EntityComponentManager::FindView(const std::set &_types, - std::map::iterator &_iter) const +std::pair EntityComponentManager::FindView( + const std::vector &_types) const { std::lock_guard lockViews(this->dataPtr->viewsMutex); - _iter = this->dataPtr->views.find(_types); - return _iter != this->dataPtr->views.end(); + std::pair viewMutexPair(nullptr, nullptr); + auto iter = this->dataPtr->views.find(_types); + if (iter != this->dataPtr->views.end()) + { + viewMutexPair.first = iter->second.first.get(); + viewMutexPair.second = iter->second.second.get(); + } + return viewMutexPair; } ////////////////////////////////////////////////// -std::map::iterator - EntityComponentManager::AddView(const std::set &_types, - detail::View &&_view) const +detail::BaseView *EntityComponentManager::AddView( + const detail::ComponentTypeKey &_types, + std::unique_ptr _view) const { // If the view already exists, then the map will return the iterator to // the location that prevented the insertion. std::lock_guard lockViews(this->dataPtr->viewsMutex); - return this->dataPtr->views.insert( - std::make_pair(_types, std::move(_view))).first; -} - -////////////////////////////////////////////////// -void EntityComponentManager::UpdateViews(const Entity _entity) -{ - IGN_PROFILE("EntityComponentManager::UpdateViews"); - for (auto &view : this->dataPtr->views) - { - // Add/update the entity if it matches the view. - if (this->EntityMatches(_entity, view.first)) - { - view.second.AddEntity(_entity, this->IsNewEntity(_entity)); - // If there is a request to delete this entity, update the view as - // well - if (this->IsMarkedForRemoval(_entity)) - { - view.second.AddEntityToRemoved(_entity); - } - for (const ComponentTypeId &compTypeId : view.first) - { - view.second.AddComponent(_entity, compTypeId, - this->EntityComponentIdFromType(_entity, compTypeId)); - } - } - else - { - view.second.RemoveEntity(_entity, view.first); - } - } + auto iter = this->dataPtr->views.insert(std::make_pair(_types, + std::make_pair(std::move(_view), + std::make_unique()))).first; + return iter->second.first.get(); } ////////////////////////////////////////////////// void EntityComponentManager::RebuildViews() { IGN_PROFILE("EntityComponentManager::RebuildViews"); - for (auto &view : this->dataPtr->views) + for (auto &viewPair : this->dataPtr->views) { - view.second.entities.clear(); - view.second.components.clear(); + auto &view = viewPair.second.first; + view->Reset(); + // Add all the entities that match the component types to the // view. for (const auto &vertex : this->dataPtr->entities.Vertices()) { Entity entity = vertex.first; - if (this->EntityMatches(entity, view.first)) + if (this->EntityMatches(entity, view->ComponentTypes())) { - view.second.AddEntity(entity, this->IsNewEntity(entity)); + view->MarkEntityToAdd(entity, this->IsNewEntity(entity)); + // If there is a request to delete this entity, update the view as // well if (this->IsMarkedForRemoval(entity)) - { - view.second.AddEntityToRemoved(entity); - } - // Store pointers to all the components. This recursively adds - // all the ComponentTypeTs that belong to the entity to the view. - for (const ComponentTypeId &compTypeId : view.first) - { - view.second.AddComponent(entity, compTypeId, - this->EntityComponentIdFromType( - entity, compTypeId)); - } + view->MarkEntityToRemove(entity); } } } @@ -877,12 +1199,12 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); - auto entRemovedComps = this->removedComponents.equal_range(_entity); - for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) + auto entRemovedCompsIter = this->removedComponents.find(_entity); + if (entRemovedCompsIter == this->removedComponents.end()) + return; + for (const auto &compType : entRemovedCompsIter->second) { - auto removedComponent = it->second; - - if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + if (!_types.empty() && _types.find(compType) == _types.end()) { continue; } @@ -891,7 +1213,7 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, // Empty data is needed for the component to be processed afterwards compMsg->set_component(" "); - compMsg->set_type(removedComponent.first); + compMsg->set_type(compType); compMsg->set_remove(true); } } @@ -902,13 +1224,16 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, const std::unordered_set &_types) { std::lock_guard lock(this->removedComponentsMutex); - uint64_t nEntityKeys = this->removedComponents.count(_entity); + auto entRemovedCompsIter = this->removedComponents.find(_entity); + if (entRemovedCompsIter == this->removedComponents.end()) + return; + uint64_t nEntityKeys = entRemovedCompsIter->second.size(); if (nEntityKeys == 0) return; // 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, + // have some removed components but none 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()) @@ -920,12 +1245,9 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, .first; } - auto entRemovedComps = this->removedComponents.equal_range(_entity); - for (auto it = entRemovedComps.first; it != entRemovedComps.second; ++it) + for (const auto &compType : entRemovedCompsIter->second) { - auto removedComponent = it->second; - - if (!_types.empty() && _types.find(removedComponent.first) == _types.end()) + if (!_types.empty() && _types.find(compType) == _types.end()) { continue; } @@ -934,11 +1256,11 @@ void EntityComponentManagerPrivate::SetRemovedComponentsMsgs(Entity &_entity, // Empty data is needed for the component to be processed afterwards compMsg.set_component(" "); - compMsg.set_type(removedComponent.first); + compMsg.set_type(compType); compMsg.set_remove(true); (*(entIter->second.mutable_components()))[ - static_cast(removedComponent.first)] = compMsg; + static_cast(compType)] = compMsg; } } @@ -948,8 +1270,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, { auto entityMsg = _msg.add_entities(); entityMsg->set_id(_entity); - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) return; if (this->dataPtr->toRemoveEntities.find(_entity) != @@ -963,9 +1285,10 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, auto types = _types; if (types.empty()) { - for (auto &type : this->dataPtr->entityComponents[_entity]) + for (auto &type : this->dataPtr->componentTypeIndex[_entity]) { - types.insert(type.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + types.insert(type.first); } } @@ -974,12 +1297,14 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedState &_msg, // If the entity does not have the component, continue auto typeIter = iter->second.find(type); if (typeIter == iter->second.end()) - { continue; - } - auto compMsg = entityMsg->add_components(); + // The component instance is nullptr if the component was removed auto compBase = this->ComponentImplementation(_entity, type); + if (nullptr == compBase) + continue; + + auto compMsg = entityMsg->add_components(); compMsg->set_type(compBase->TypeId()); std::ostringstream ostr; @@ -998,8 +1323,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, Entity _entity, const std::unordered_set &_types, bool _full) const { - auto iter = this->dataPtr->entityComponents.find(_entity); - if (iter == this->dataPtr->entityComponents.end()) + auto iter = this->dataPtr->componentTypeIndex.find(_entity); + if (iter == this->dataPtr->componentTypeIndex.end()) return; // Set the default entity iterator to the end. This will allow us to know @@ -1028,9 +1353,10 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, auto types = _types; if (types.empty()) { - for (auto &type : this->dataPtr->entityComponents[_entity]) + for (auto &type : this->dataPtr->componentTypeIndex[_entity]) { - types.insert(type.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + types.insert(type.first); } } @@ -1046,16 +1372,30 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, const components::BaseComponent *compBase = this->ComponentImplementation(_entity, type); - ComponentKey comp = {type, typeIter->second}; - // If not sending full state, skip unchanged components - if (!_full && - this->dataPtr->oneTimeChangedComponents.find(comp) == - this->dataPtr->oneTimeChangedComponents.end() && - this->dataPtr->periodicChangedComponents.find(comp) == - this->dataPtr->periodicChangedComponents.end()) + if (!_full) { - continue; + bool noChange = true; + + // see if the entity has a component of this particular type marked as a + // one time change + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end() && + oneTimeIter->second.find(_entity) != oneTimeIter->second.end()) + noChange = false; + + if (noChange) + { + // see if the entity has a component of this particular type marked as a + // periodic change + auto periodicIter = this->dataPtr->periodicChangedComponents.find(type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end() && + periodicIter->second.find(_entity) != oneTimeIter->second.end()) + noChange = false; + } + + if (noChange) + continue; } /// Find the entity in the message, if not already found. @@ -1072,7 +1412,7 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, } } - auto compIter = entIter->second.mutable_components()->find(comp.first); + auto compIter = entIter->second.mutable_components()->find(type); // Find the component in the message, and add the component to the // message if it's not present. if (compIter == entIter->second.mutable_components()->end()) @@ -1080,8 +1420,8 @@ void EntityComponentManager::AddEntityToMessage(msgs::SerializedStateMap &_msg, msgs::SerializedComponent cmp; cmp.set_type(compBase->TypeId()); (*(entIter->second.mutable_components()))[ - static_cast(comp.first)] = cmp; - compIter = entIter->second.mutable_components()->find(comp.first); + static_cast(type)] = cmp; + compIter = entIter->second.mutable_components()->find(type); } // Serialize and store the message @@ -1148,44 +1488,44 @@ void EntityComponentManager::ChangedState( void EntityComponentManagerPrivate::CalculateStateThreadLoad() { // If the entity component vector is dirty, we need to recalculate the - // threads and each threads work load - if (!this->entityComponentsDirty) + // threads and each thread's work load + if (!this->componentTypeIndexDirty) return; - this->entityComponentsDirty = false; - this->entityComponentIterators.clear(); - auto startIt = this->entityComponents.begin(); - int numComponents = this->entityComponents.size(); + this->componentTypeIndexDirty = false; + this->componentTypeIndexIterators.clear(); + auto startIt = this->componentTypeIndex.begin(); + int numEntities = this->componentTypeIndex.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); + uint64_t numThreads = std::min(numEntities, maxThreads); - int componentsPerThread = static_cast(std::ceil( - static_cast(numComponents) / numThreads)); + int entitiesPerThread = static_cast(std::ceil( + static_cast(numEntities) / numThreads)); igndbg << "Updated state thread iterators: " << numThreads - << " threads processing around " << componentsPerThread - << " components each." << std::endl; + << " threads processing around " << entitiesPerThread + << " entities each." << std::endl; // Push back the starting iterator - this->entityComponentIterators.push_back(startIt); + this->componentTypeIndexIterators.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 + // If we have added all of the entities to the iterator vector, we are // done so push back the end iterator - numComponents -= componentsPerThread; - if (numComponents <= 0) + numEntities -= entitiesPerThread; + if (numEntities <= 0) { - this->entityComponentIterators.push_back( - this->entityComponents.end()); + this->componentTypeIndexIterators.push_back( + this->componentTypeIndex.end()); break; } - // Get the iterator to the next starting group of components - auto nextIt = std::next(startIt, componentsPerThread); - this->entityComponentIterators.push_back(nextIt); + // Get the iterator to the next starting group of entities + auto nextIt = std::next(startIt, entitiesPerThread); + this->componentTypeIndexIterators.push_back(nextIt); startIt = nextIt; } } @@ -1196,7 +1536,7 @@ ignition::msgs::SerializedState EntityComponentManager::State( const std::unordered_set &_types) const { ignition::msgs::SerializedState stateMsg; - for (const auto &it : this->dataPtr->entityComponents) + for (const auto &it : this->dataPtr->componentTypeIndex) { auto entity = it.first; if (!_entities.empty() && _entities.find(entity) == _entities.end()) @@ -1244,12 +1584,12 @@ void EntityComponentManager::State( }; // Spawn workers - uint64_t numThreads = this->dataPtr->entityComponentIterators.size() - 1; + uint64_t numThreads = this->dataPtr->componentTypeIndexIterators.size() - 1; for (uint64_t i = 0; i < numThreads; i++) { workers.push_back(std::thread(functor, - this->dataPtr->entityComponentIterators[i], - this->dataPtr->entityComponentIterators[i+1])); + this->dataPtr->componentTypeIndexIterators[i], + this->dataPtr->componentTypeIndexIterators[i+1])); } // Wait for each thread to finish processing its components @@ -1313,53 +1653,36 @@ void EntityComponentManager::SetState( continue; } - // Create component - auto newComp = components::Factory::Instance()->New(compMsg.type()); - - if (nullptr == newComp) - { - ignerr << "Failed to deserialize component of type [" << compMsg.type() - << "]" << std::endl; - continue; - } - - std::istringstream istr(compMsg.component()); - newComp->Deserialize(istr); - - // Get type id - auto typeId = newComp->TypeId(); - - // TODO(louise) Move into if, see TODO below - this->RemoveComponent(entity, typeId); - // Remove component if (compMsg.remove()) { + this->RemoveComponent(entity, type); continue; } // Get Component - auto comp = this->ComponentImplementation(entity, typeId); + auto comp = this->ComponentImplementation(entity, type); + + std::istringstream istr(compMsg.component()); // Create if new if (nullptr == comp) { - this->CreateComponentImplementation(entity, typeId, newComp.get()); + auto newComp = components::Factory::Instance()->New(type); + if (nullptr == newComp) + { + ignerr << "Failed to create component type [" + << compMsg.type() << "]" << std::endl; + continue; + } + newComp->Deserialize(istr); + this->CreateComponentImplementation(entity, type, newComp.get()); } // Update component value else { - ignerr << "Internal error" << std::endl; - // TODO(louise) We're shortcutting above and always removing the - // component so that we don't get here, gotta figure out why this - // doesn't update the component. The following line prints the correct - // values. - // igndbg << *comp << " " << *newComp.get() << std::endl; - // *comp = *newComp.get(); - - // When above TODO is addressed, uncomment AddModifiedComponent below - // unless calling SetChanged (which already calls AddModifiedComponent) - // this->dataPtr->AddModifiedComponent(entity); + comp->Deserialize(istr); + this->dataPtr->AddModifiedComponent(entity); } } } @@ -1494,31 +1817,38 @@ void EntityComponentManager::SetChanged( const Entity _entity, const ComponentTypeId _type, gazebo::ComponentState _c) { - auto ecIter = this->dataPtr->entityComponents.find(_entity); - - if (ecIter == this->dataPtr->entityComponents.end()) + // make sure _entity exists + auto ecIter = this->dataPtr->componentTypeIndex.find(_entity); + if (ecIter == this->dataPtr->componentTypeIndex.end()) return; - auto typeIter = ecIter->second.find(_type); - if (typeIter == ecIter->second.end()) + // make sure the entity has a component of type _type + if (ecIter->second.find(_type) == ecIter->second.end() || + this->dataPtr->ComponentMarkedAsRemoved(_entity, _type)) return; - ComponentKey key{_type, typeIter->second}; - if (_c == ComponentState::PeriodicChange) { - this->dataPtr->periodicChangedComponents.insert(key); - this->dataPtr->oneTimeChangedComponents.erase(key); + this->dataPtr->periodicChangedComponents[_type].insert(_entity); + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + oneTimeIter->second.erase(_entity); } else if (_c == ComponentState::OneTimeChange) { - this->dataPtr->periodicChangedComponents.erase(key); - this->dataPtr->oneTimeChangedComponents.insert(key); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + periodicIter->second.erase(_entity); + this->dataPtr->oneTimeChangedComponents[_type].insert(_entity); } else { - this->dataPtr->periodicChangedComponents.erase(key); - this->dataPtr->oneTimeChangedComponents.erase(key); + auto periodicIter = this->dataPtr->periodicChangedComponents.find(_type); + if (periodicIter != this->dataPtr->periodicChangedComponents.end()) + periodicIter->second.erase(_entity); + auto oneTimeIter = this->dataPtr->oneTimeChangedComponents.find(_type); + if (oneTimeIter != this->dataPtr->oneTimeChangedComponents.end()) + oneTimeIter->second.erase(_entity); } this->dataPtr->AddModifiedComponent(_entity); @@ -1528,15 +1858,15 @@ void EntityComponentManager::SetChanged( std::unordered_set EntityComponentManager::ComponentTypes( const Entity _entity) const { - std::unordered_set result; + auto it = this->dataPtr->componentTypeIndex.find(_entity); + if (it == this->dataPtr->componentTypeIndex.end()) + return {}; - auto it = this->dataPtr->entityComponents.find(_entity); - if (it == this->dataPtr->entityComponents.end()) - return result; - - for (const auto &key : it->second) + std::unordered_set result; + for (const auto &type : it->second) { - result.insert(key.first); + if (!this->dataPtr->ComponentMarkedAsRemoved(_entity, type.first)) + result.insert(type.first); } return result; @@ -1555,6 +1885,18 @@ void EntityComponentManager::SetEntityCreateOffset(uint64_t _offset) this->dataPtr->entityCount = _offset; } +///////////////////////////////////////////////// +void EntityComponentManager::LockAddingEntitiesToViews(bool _lock) +{ + this->dataPtr->lockAddEntitiesToViews = _lock; +} + +///////////////////////////////////////////////// +bool EntityComponentManager::LockAddingEntitiesToViews() const +{ + return this->dataPtr->lockAddEntitiesToViews; +} + ///////////////////////////////////////////////// void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) { @@ -1571,6 +1913,51 @@ void EntityComponentManagerPrivate::AddModifiedComponent(const Entity &_entity) this->modifiedComponents.insert(_entity); } +///////////////////////////////////////////////// +bool EntityComponentManagerPrivate::ComponentMarkedAsRemoved( + const Entity _entity, const ComponentTypeId _typeId) const +{ + auto iter = this->componentsMarkedAsRemoved.find(_entity); + if (iter != this->componentsMarkedAsRemoved.end()) + return iter->second.find(_typeId) != iter->second.end(); + + return false; +} + +///////////////////////////////////////////////// +template +bool EntityComponentManagerPrivate::ClonedJointLinkName(Entity _joint, + Entity _originalLink, EntityComponentManager *_ecm) +{ + if (ComponentTypeT::typeId != components::ParentLinkName::typeId && + ComponentTypeT::typeId != components::ChildLinkName::typeId) + { + ignerr << "Template type is invalid. Must be either " + << "components::ParentLinkName or components::ChildLinkName\n"; + return false; + } + + auto iter = this->originalToClonedLink.find(_originalLink); + if (iter == this->originalToClonedLink.end()) + { + ignerr << "Error: attempted to clone links, but link [" + << _originalLink << "] was never cloned.\n"; + return false; + } + auto clonedLink = iter->second; + + auto name = _ecm->Component(clonedLink); + if (!name) + { + ignerr << "Link [" << _originalLink << "] was cloned, but its clone has no " + << "name.\n"; + return false; + } + + _ecm->SetComponentData(_joint, name->Data()); + return true; +} + ///////////////////////////////////////////////// void EntityComponentManager::PinEntity(const Entity _entity, bool _recursive) { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 29d271fc79e..ed45452d669 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -21,8 +21,16 @@ #include #include #include +#include +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Factory.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/config.hh" @@ -105,149 +113,24 @@ class EntityComponentManagerFixture public: EntityCompMgrTest manager; }; -///////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, AdjacentMemorySingleComponentType) -{ - std::vector poses; - std::vector keys; - - int count = 10; - - Entity entity = manager.CreateEntity(); - EXPECT_EQ(1u, entity); - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d( - math::Rand::IntNormal(10, 5), - math::Rand::IntNormal(100, 50), - math::Rand::IntNormal(-100, 30), 0, 0, 0))); - keys.push_back(manager.CreateComponent(entity, poses.back())); - - // The component ids should increment by one for each component. - EXPECT_EQ(keys.back().second, i); - } - - ASSERT_EQ(count, static_cast(poses.size())); - ASSERT_EQ(count, static_cast(keys.size())); - - // Check the component values. - for (int i = 0; i < count; ++i) - { - EXPECT_EQ(poses[i], *(manager.Component(keys[i]))); - } - { - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(keys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - prevPose = pose; - } - } - { - // Check that the data member of each Component is adjacent in memory - const math::Pose3d *poseData = nullptr, *prevPoseData = nullptr; - for (int i = 0; i < count; ++i) - { - poseData = &(manager.Component(keys[i])->Data()); - uintptr_t poseDataSize = sizeof(math::Pose3d) + - sizeof(components::BaseComponent); - if (prevPoseData != nullptr) - { - EXPECT_EQ(poseDataSize, reinterpret_cast(poseData) - - reinterpret_cast(prevPoseData)); - } - prevPoseData = poseData; - } - } -} - -///////////////////////////////////////////////// -TEST_P(EntityComponentManagerFixture, AdjacentMemoryTwoComponentTypes) -{ - common::setenv("IGN_DEBUG_COMPONENT_FACTORY", "true"); - - std::vector poses; - std::vector ints; - std::vector poseKeys; - std::vector intKeys; - - int count = 100000; - - Entity entity = manager.CreateEntity(); - EXPECT_EQ(1u, entity); - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); - ints.push_back(IntComponent(i)); - - poseKeys.push_back(manager.CreateComponent(entity, poses.back())); - intKeys.push_back(manager.CreateComponent(entity, ints.back())); - - // The component ids should increment by one for each component. - EXPECT_EQ(poseKeys.back().second, i); - EXPECT_EQ(intKeys.back().second, i); - } - - ASSERT_EQ(static_cast(count), poses.size()); - ASSERT_EQ(static_cast(count), ints.size()); - ASSERT_EQ(static_cast(count), poseKeys.size()); - ASSERT_EQ(static_cast(count), intKeys.size()); - - uintptr_t poseSize = sizeof(components::Pose); - uintptr_t intSize = sizeof(IntComponent); - const components::Pose *pose = nullptr, *prevPose = nullptr; - const IntComponent *it = nullptr, *prevIt = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(poseKeys[i]); - it = manager.Component(intKeys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - - if (prevIt != nullptr) - { - EXPECT_EQ(intSize, reinterpret_cast(it) - - reinterpret_cast(prevIt)); - } - prevPose = pose; - prevIt = it; - } -} - ///////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, InvalidComponentType) { - ComponentKey key{999, 0}; - // Can't remove component from an nonexistent entity EXPECT_FALSE(manager.HasEntity(2)); - EXPECT_FALSE(manager.RemoveComponent(2, key)); + EXPECT_FALSE(manager.RemoveComponent(2, IntComponent::typeId)); // Can't remove a component that doesn't exist. EXPECT_EQ(1u, manager.CreateEntity()); EXPECT_EQ(2u, manager.CreateEntity()); EXPECT_TRUE(manager.HasEntity(2)); - EXPECT_FALSE(manager.RemoveComponent(2, key)); + EXPECT_FALSE(manager.RemoveComponent(2, IntComponent::typeId)); // We should get a nullptr if the component type doesn't exist. - EXPECT_EQ(nullptr, manager.Component(key)); + EXPECT_TRUE(manager.HasEntity(1u)); + EXPECT_TRUE(manager.HasEntity(2u)); + EXPECT_EQ(nullptr, manager.Component(1u)); + EXPECT_EQ(nullptr, manager.Component(2u)); } ///////////////////////////////////////////////// @@ -259,162 +142,71 @@ TEST_P(EntityComponentManagerFixture, RemoveComponent) auto eIntDouble = manager.CreateEntity(); EXPECT_EQ(3u, manager.EntityCount()); - // Add components and keep their unique ComponentKeys + // Add components auto cIntEInt = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, cIntEInt); auto cDoubleEDouble = manager.CreateComponent(eDouble, DoubleComponent(0.123)); + ASSERT_NE(nullptr, cDoubleEDouble); auto cIntEIntDouble = manager.CreateComponent(eIntDouble, IntComponent(456)); + ASSERT_NE(nullptr, cIntEIntDouble); auto cDoubleEIntDouble = manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + ASSERT_NE(nullptr, cDoubleEIntDouble); // Check entities have the components - EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_TRUE(manager.EntityHasComponentType(eInt, IntComponent::typeId)); EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); + EXPECT_EQ(cIntEInt, manager.Component(eInt)); - EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.EntityHasComponentType(eDouble, DoubleComponent::typeId)); EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); + EXPECT_EQ(cDoubleEDouble, manager.Component(eDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); auto types = manager.ComponentTypes(eIntDouble); EXPECT_NE(types.end(), types.find(IntComponent::typeId)); EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); - - // Remove component by key - EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); - EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); - EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + EXPECT_EQ(cIntEIntDouble, manager.Component(eIntDouble)); + EXPECT_EQ(cDoubleEIntDouble, manager.Component(eIntDouble)); // Remove component by type id - auto typeDouble = DoubleComponent::typeId; + EXPECT_TRUE(manager.RemoveComponent(eInt, IntComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eInt, IntComponent::typeId)); + EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + EXPECT_EQ(nullptr, manager.Component(eInt)); - EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.RemoveComponent(eDouble, DoubleComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eDouble, + DoubleComponent::typeId)); EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); + EXPECT_EQ(nullptr, manager.Component(eDouble)); // Remove component by type EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); - EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + IntComponent::typeId)); + EXPECT_TRUE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); + types = manager.ComponentTypes(eIntDouble); + EXPECT_EQ(1u, types.size()); + EXPECT_EQ(types.end(), types.find(IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(eIntDouble)); EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); - EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + IntComponent::typeId)); + EXPECT_FALSE(manager.EntityHasComponentType(eIntDouble, + DoubleComponent::typeId)); EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); -} - -///////////////////////////////////////////////// -// Removing a component should guarantee that existing components remain -// adjacent to each other. -TEST_P(EntityComponentManagerFixture, RemoveAdjacent) -{ - std::vector poses; - std::vector keys; - - Entity entity = manager.CreateEntity(); - - int count = 3; - - // Create the components. - for (int i = 0; i < count; ++i) - { - poses.push_back(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0))); - keys.push_back(manager.CreateComponent(entity, poses.back())); - EXPECT_EQ(keys.back().second, i); - } - ASSERT_EQ(poses.size(), keys.size()); - - uintptr_t poseSize = sizeof(components::Pose); - const components::Pose *pose = nullptr, *prevPose = nullptr; - - // Check that each component is adjacent in memory - for (int i = 0; i < count; ++i) - { - pose = manager.Component(keys[i]); - if (prevPose != nullptr) - { - EXPECT_EQ(poseSize, reinterpret_cast(pose) - - reinterpret_cast(prevPose)); - } - prevPose = pose; - } - - // Remove the middle component. - EXPECT_TRUE(manager.EntityHasComponent(entity, keys[1])); - EXPECT_TRUE(manager.RemoveComponent(entity, keys[1])); - EXPECT_FALSE(manager.EntityHasComponent(entity, keys[1])); - - // Can't remove the component twice. - EXPECT_FALSE(manager.RemoveComponent(entity, keys[1])); - - // Check that the two remaining components are still adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose3 = - manager.Component(keys[2]); - EXPECT_EQ(poseSize, - reinterpret_cast(pose3) - reinterpret_cast(pose1)); -} - -///////////////////////////////////////////////// -// Removing a component should guarantee that existing components remain -// adjacent to each other, and addition of a new component is adjacent to -// the last element. -TEST_P(EntityComponentManagerFixture, RemoveAddAdjacent) -{ - Entity entity = manager.CreateEntity(); - - std::vector keys; - - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)))); - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(3, 1, 2, 0, 0, 0)))); - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)))); - - uintptr_t poseSize = sizeof(components::Pose); - - // Remove the middle component. - EXPECT_TRUE(manager.RemoveComponent(entity, keys[1])); - - // Add two more new component - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)))); - - keys.push_back(manager.CreateComponent(entity, - components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)))); - - // Check that the components are all adjacent in memory - const components::Pose *pose1 = - manager.Component(keys[0]); - const components::Pose *pose2 = - manager.Component(keys[2]); - const components::Pose *pose3 = - manager.Component(keys[3]); - const components::Pose *pose4 = - manager.Component(keys[4]); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose2) - reinterpret_cast(pose1)); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose3) - reinterpret_cast(pose2)); - - EXPECT_EQ(poseSize, - reinterpret_cast(pose4) - reinterpret_cast(pose3)); - - // Check the values of the components. - EXPECT_EQ(components::Pose(math::Pose3d(1, 2, 3, 0, 0, 0)), *pose1); - EXPECT_EQ(components::Pose(math::Pose3d(0, 10, 20, 0, 0, 0)), *pose2); - EXPECT_EQ(components::Pose(math::Pose3d(101, 51, 520, 0, 0, 0)), *pose3); - EXPECT_EQ(components::Pose(math::Pose3d(1010, 81, 821, 0, 0, 0)), *pose4); + EXPECT_EQ(nullptr, manager.Component(eIntDouble)); } ///////////////////////////////////////////////// @@ -429,12 +221,13 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add a component to an entity - ComponentKey cKey = manager.CreateComponent(entity, + auto compPtr = manager.CreateComponent(entity, IntComponent(123)); + ASSERT_NE(nullptr, compPtr); EXPECT_TRUE(manager.HasComponentType(IntComponent::typeId)); - EXPECT_TRUE(manager.EntityHasComponent(entity, cKey)); EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); + EXPECT_EQ(compPtr, manager.Component(entity)); EXPECT_FALSE(manager.EntityHasComponentType(entity, DoubleComponent::typeId)); EXPECT_FALSE(manager.EntityHasComponentType(entity2, IntComponent::typeId)); @@ -442,7 +235,7 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_FALSE(manager.HasEntity(kNullEntity)); EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, IntComponent::typeId)); - EXPECT_EQ(ComponentKey(), manager.CreateComponent(kNullEntity, + EXPECT_EQ(nullptr, manager.CreateComponent(kNullEntity, IntComponent(123))); EXPECT_FALSE(manager.HasEntity(kNullEntity)); EXPECT_FALSE(manager.EntityHasComponentType(kNullEntity, @@ -473,6 +266,21 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_TRUE(manager.EntityHasComponentType(entity, IntComponent::typeId)); EXPECT_EQ(123, intComp->Data()); + // Try to create/query a component from an entity that does not exist. nullptr + // should be returned since a component cannot be attached to a non-existent + // entity + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + EXPECT_EQ(nullptr, manager.CreateComponent(kNullEntity, + IntComponent(123))); + EXPECT_EQ(nullptr, manager.ComponentDefault(kNullEntity, 123)); + EXPECT_EQ(nullptr, manager.Component(kNullEntity)); + EXPECT_FALSE(manager.ComponentData(kNullEntity).has_value()); + EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(kNullEntity, + IntComponent::typeId)); + // (make sure the entity wasn't implicitly created during the invalid + // component calls) + EXPECT_FALSE(manager.HasEntity(kNullEntity)); + // Remove all entities manager.RequestRemoveEntities(); EXPECT_EQ(3u, manager.EntityCount()); @@ -482,7 +290,6 @@ TEST_P(EntityComponentManagerFixture, EntitiesAndComponents) EXPECT_EQ(0u, manager.EntityCount()); EXPECT_FALSE(manager.HasEntity(entity)); EXPECT_FALSE(manager.HasEntity(entity2)); - EXPECT_FALSE(manager.EntityHasComponent(entity, cKey)); EXPECT_FALSE(manager.EntityHasComponentType(entity, IntComponent::typeId)); // The type itself still exists @@ -501,14 +308,23 @@ TEST_P(EntityComponentManagerFixture, ComponentValues) EXPECT_EQ(5u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); - manager.CreateComponent(ePose, + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(ePose, components::Pose({1, 2, 3, 0, 0, 0})); - manager.CreateComponent(eCustom, + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(eCustom, CustomComponent(Custom())); + ASSERT_NE(nullptr, comp6); // Get and set component values { @@ -665,10 +481,17 @@ TEST_P(EntityComponentManagerFixture, RebuildViews) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); // The first iteration of this loop builds views. At the end, views are // rebuilt. The second iteration should return the same values as the @@ -727,10 +550,17 @@ TEST_P(EntityComponentManagerFixture, ViewsAddComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); for (int i = 0; i < 2; ++i) { @@ -777,7 +607,9 @@ TEST_P(EntityComponentManagerFixture, ViewsAddComponents) else EXPECT_EQ(3, count); - manager.CreateComponent(eInt, DoubleComponent(12.123)); + auto comp5 = manager.CreateComponent(eInt, + DoubleComponent(12.123)); + ASSERT_NE(nullptr, comp5); } } @@ -791,11 +623,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); auto compToRemove = manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + ASSERT_NE(nullptr, compToRemove); for (int i = 0; i < 2; ++i) { @@ -844,7 +682,7 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveComponents) if (i == 0) { - EXPECT_TRUE(manager.RemoveComponent(eIntDouble, compToRemove)); + EXPECT_TRUE(manager.RemoveComponent(eIntDouble, compToRemove->TypeId())); } } } @@ -859,10 +697,17 @@ TEST_P(EntityComponentManagerFixture, ViewsAddEntity) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); Entity newEntity; @@ -924,7 +769,10 @@ TEST_P(EntityComponentManagerFixture, ViewsAddEntity) EXPECT_EQ(2, count); newEntity = manager.CreateEntity(); - manager.CreateComponent(newEntity, IntComponent(789)); + ASSERT_NE(kNullEntity, newEntity); + auto createdComp = manager.CreateComponent(newEntity, + IntComponent(789)); + ASSERT_NE(nullptr, createdComp); } } @@ -938,10 +786,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveEntities) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); for (int i = 0; i < 2; ++i) { @@ -1067,10 +922,17 @@ TEST_P(EntityComponentManagerFixture, ViewsRemoveEntity) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(123)); - manager.CreateComponent(eDouble, DoubleComponent(0.123)); - manager.CreateComponent(eIntDouble, IntComponent(456)); - manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); + auto comp1 = manager.CreateComponent(eInt, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eDouble, + DoubleComponent(0.123)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(eIntDouble, + IntComponent(456)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eIntDouble, + DoubleComponent(0.456)); + ASSERT_NE(nullptr, comp4); int count = 0; manager.Each ([&](const Entity &_entity, @@ -1191,8 +1053,10 @@ TEST_P(EntityComponentManagerFixture, EachNewBasic) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2, newCount(manager)); EXPECT_TRUE(manager.HasNewEntities()); @@ -1202,6 +1066,26 @@ TEST_P(EntityComponentManagerFixture, EachNewBasic) manager.RunClearNewlyCreatedEntities(); EXPECT_EQ(0, newCount(manager)); EXPECT_FALSE(manager.HasNewEntities()); + + // Below tests adding a new entity, but not using the view until the following + // simulation step. This is to ensure that the views update the status of + // their new entities correctly at the end of each simulation step, regardless + // of whether the view is used in a given simulation step or not + + // Create a new entity and add a component to it that makes this entity a + // part of the IntComponent View + Entity e3 = manager.CreateEntity(); + EXPECT_EQ(3u, manager.EntityCount()); + EXPECT_TRUE(manager.HasNewEntities()); + auto comp3 = manager.CreateComponent(e3, IntComponent(789)); + EXPECT_NE(nullptr, comp3); + // Mimic the end of a simulation step + manager.RunClearNewlyCreatedEntities(); + // Use the IntComponent View, checking that the view has no entities marked as + // new since we are now in a new simulation step + EXPECT_EQ(0, newCount(manager)); + EXPECT_FALSE(manager.HasNewEntities()); + EXPECT_EQ(3, eachCount(manager)); } ////////////////////////////////////////////////// @@ -1210,11 +1094,14 @@ TEST_P(EntityComponentManagerFixture, EachNewAfterRemoveComponent) // Create entities Entity e1 = manager.CreateEntity(); auto comp1 = manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, newCount(manager)); - manager.RemoveComponent(e1, comp1); + EXPECT_TRUE(manager.RemoveComponent(e1, comp1->TypeId())); EXPECT_EQ(1, newCount(manager)); manager.RunClearNewlyCreatedEntities(); @@ -1226,13 +1113,15 @@ TEST_P(EntityComponentManagerFixture, EachNewRemoveComponentFromRemoveEntity) { // Create entities Entity e1 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); manager.RunClearNewlyCreatedEntities(); // Nothing new after cleared EXPECT_EQ(0, newCount(manager)); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e2, IntComponent(456)); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, newCount(manager)); // Check if this true after RebuildViews manager.RebuildViews(); @@ -1245,18 +1134,25 @@ TEST_P(EntityComponentManagerFixture, EachNewAddComponentToExistingEntity) // Create entities Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); manager.RunClearNewlyCreatedEntities(); // Nothing new after cleared EXPECT_EQ(0, newCount(manager)); // Create a new entity Entity e3 = manager.CreateEntity(); - manager.CreateComponent(e3, IntComponent(789)); + auto comp3 = manager.CreateComponent(e3, IntComponent(789)); + ASSERT_NE(nullptr, comp3); // Add a new component to existing entities - manager.CreateComponent(e1, DoubleComponent(0.0)); - manager.CreateComponent(e2, DoubleComponent(2.0)); + auto comp4 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(e2, + DoubleComponent(2.0)); + ASSERT_NE(nullptr, comp5); // e1 and e2 have a new double component, but they are not considered new // entities @@ -1274,8 +1170,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveBasic) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); // Remove an entity. manager.RequestRemoveEntity(e1); @@ -1304,8 +1202,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAlreadyRemove) EXPECT_EQ(2u, manager.EntityCount()); // Add components to each entity - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); manager.RequestRemoveEntity(e2); manager.ProcessEntityRemovals(); @@ -1322,7 +1222,8 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAfterRebuild) Entity e1 = manager.CreateEntity(); EXPECT_EQ(1u, manager.EntityCount()); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); EXPECT_EQ(1, newCount(manager)); manager.RunClearNewlyCreatedEntities(); @@ -1337,13 +1238,16 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAfterRebuild) TEST_P(EntityComponentManagerFixture, EachRemoveAddComponentToRemoveEntity) { Entity e1 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); manager.RunClearNewlyCreatedEntities(); manager.RequestRemoveEntity(e1); // Add a new component to an removed entity. This should be possible since the // entity is only scheduled to be removed. - manager.CreateComponent(e1, DoubleComponent(0.0)); + auto comp2 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(1, removedCount(manager)); EXPECT_EQ(1, (removedCount(manager))); } @@ -1354,8 +1258,10 @@ TEST_P(EntityComponentManagerFixture, EachRemoveAllRemove) // Test when all entities are removed Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); manager.RequestRemoveEntities(); @@ -1371,8 +1277,10 @@ TEST_P(EntityComponentManagerFixture, EachNewEachRemove) // Test EachNew and EachRemove together Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_EQ(2, newCount(manager)); @@ -1399,8 +1307,10 @@ TEST_P(EntityComponentManagerFixture, EachGetsNewOldRemove) // Test that an Each call gets new, old, and removed entities Entity e1 = manager.CreateEntity(); Entity e2 = manager.CreateEntity(); - manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e2, IntComponent(456)); + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, comp2); EXPECT_EQ(2u, manager.EntityCount()); EXPECT_EQ(2, eachCount(manager)); @@ -1426,6 +1336,66 @@ TEST_P(EntityComponentManagerFixture, EachGetsNewOldRemove) EXPECT_EQ(0, removedCount(manager)); } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, EachAddRemoveComponent) +{ + // test calling ecm.Each on entities that have components added/removed + // frequently. This is common with *Cmd components + + Entity e1 = manager.CreateEntity(); + EXPECT_EQ(1u, manager.EntityCount()); + EXPECT_EQ(0, eachCount(manager)); + auto comp = manager.Component(e1); + EXPECT_EQ(nullptr, comp); + + // add a component + auto comp1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, comp1); + EXPECT_EQ(1, eachCount(manager)); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(123, comp->Data()); + EXPECT_EQ(123, comp1->Data()); + EXPECT_EQ(comp, comp1); + + // remove a component + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + EXPECT_EQ(0, eachCount(manager)); + + // add the same type of component back in + auto comp2 = manager.CreateComponent(e1, IntComponent(456)); + ASSERT_NE(nullptr, comp2); + EXPECT_EQ(1, eachCount(manager)); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(456, comp->Data()); + EXPECT_EQ(456, comp2->Data()); + EXPECT_EQ(comp, comp2); + + // remove the component again + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + EXPECT_EQ(0, eachCount(manager)); + + // add and remove the component before calling ecm.Each. This is to make sure + // that the views remove any entities in their toAddEntities queue if required + // components for entities in a view's toAddEntities queue are removed before + // calling Each, since a view's toAddEntities queue is processed (and then + // cleared) in an Each call + auto comp3 = manager.CreateComponent(e1, IntComponent(789)); + ASSERT_NE(nullptr, comp3); + comp = manager.Component(e1); + ASSERT_NE(nullptr, comp); + EXPECT_EQ(789, comp->Data()); + EXPECT_EQ(789, comp3->Data()); + EXPECT_EQ(comp, comp3); + EXPECT_TRUE(manager.RemoveComponent(e1, IntComponent::typeId)); + EXPECT_EQ(nullptr, manager.Component(e1)); + // call ecm.Each after adding/removing the component + EXPECT_EQ(0, eachCount(manager)); +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, EntityByComponents) { @@ -1436,16 +1406,28 @@ TEST_P(EntityComponentManagerFixture, EntityByComponents) EXPECT_EQ(3u, manager.EntityCount()); // Add components of different types to each entity - manager.CreateComponent(eInt, IntComponent(-123)); - manager.CreateComponent(eInt, StringComponent("int")); - - manager.CreateComponent(eUint, UIntComponent(456u)); - manager.CreateComponent(eUint, StringComponent("uint")); - - manager.CreateComponent(eIntUint, IntComponent(789)); - manager.CreateComponent(eIntUint, UIntComponent(789u)); - manager.CreateComponent(eIntUint, + auto comp1 = manager.CreateComponent(eInt, IntComponent(-123)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(eInt, + StringComponent("int")); + ASSERT_NE(nullptr, comp2); + + auto comp3 = manager.CreateComponent(eUint, + UIntComponent(456u)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(eUint, + StringComponent("uint")); + ASSERT_NE(nullptr, comp4); + + auto comp5 = manager.CreateComponent(eIntUint, + IntComponent(789)); + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(eIntUint, + UIntComponent(789u)); + ASSERT_NE(nullptr, comp6); + auto comp7 = manager.CreateComponent(eIntUint, StringComponent("int-uint")); + ASSERT_NE(nullptr, comp7); // Get entities by the value of their components EXPECT_EQ(eInt, manager.EntityByComponents(IntComponent(-123))); @@ -1481,8 +1463,11 @@ TEST_P(EntityComponentManagerFixture, EntityByComponents) Entity eInt2 = manager.CreateEntity(); EXPECT_EQ(4u, manager.EntityCount()); - manager.CreateComponent(eInt2, IntComponent(-123)); - manager.CreateComponent(eInt2, StringComponent("int2")); + auto comp8 = manager.CreateComponent(eInt2, IntComponent(-123)); + ASSERT_NE(nullptr, comp8); + auto comp9 = manager.CreateComponent(eInt2, + StringComponent("int2")); + ASSERT_NE(nullptr, comp9); auto entities = manager.EntitiesByComponents(IntComponent(-123)); EXPECT_EQ(2u, entities.size()); @@ -1550,14 +1535,21 @@ TEST_P(EntityComponentManagerFixture, EntityGraph) */ // Add components - manager.CreateComponent(e2, {}); - manager.CreateComponent(e4, {}); - manager.CreateComponent(e6, {}); - - manager.CreateComponent(e1, {}); - manager.CreateComponent(e3, {}); - manager.CreateComponent(e5, {}); - manager.CreateComponent(e7, {}); + auto comp1 = manager.CreateComponent(e2, {}); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e4, {}); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(e6, {}); + ASSERT_NE(nullptr, comp3); + + auto comp4 = manager.CreateComponent(e1, {}); + ASSERT_NE(nullptr, comp4); + auto comp5 = manager.CreateComponent(e3, {}); + ASSERT_NE(nullptr, comp5); + auto comp6 = manager.CreateComponent(e5, {}); + ASSERT_NE(nullptr, comp6); + auto comp7 = manager.CreateComponent(e7, {}); + ASSERT_NE(nullptr, comp7); // Get children by components { @@ -1649,10 +1641,16 @@ TEST_P(EntityComponentManagerFixture, State) EXPECT_EQ(3u, manager.EntityCount()); // Components - manager.CreateComponent(e1, IntComponent(e1c0)); - manager.CreateComponent(e2, DoubleComponent(e2c0)); - manager.CreateComponent(e2, StringComponent(e2c1)); - manager.CreateComponent(e3, IntComponent(e3c0)); + auto comp1 = manager.CreateComponent(e1, IntComponent(e1c0)); + ASSERT_NE(nullptr, comp1); + auto comp2 = manager.CreateComponent(e2, + DoubleComponent(e2c0)); + ASSERT_NE(nullptr, comp2); + auto comp3 = manager.CreateComponent(e2, + StringComponent(e2c1)); + ASSERT_NE(nullptr, comp3); + auto comp4 = manager.CreateComponent(e3, IntComponent(e3c0)); + ASSERT_NE(nullptr, comp4); } // Serialize into a message @@ -1911,7 +1909,8 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(1u, manager.EntityCount()); // Component - manager.CreateComponent(e1, IntComponent(e1c0)); + auto comp1 = manager.CreateComponent(e1, IntComponent(e1c0)); + ASSERT_NE(nullptr, comp1); // Serialize into a message msgs::SerializedStateMap stateMsg; @@ -1924,8 +1923,9 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(0, changedStateMsg.entities_size()); // create component - auto compKey = + auto compPtr = manager.CreateComponent(e1, StringComponent(e1c1)); + ASSERT_NE(nullptr, compPtr); changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); manager.State(stateMsg); @@ -1941,7 +1941,7 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) msgs::SerializedEntityMap &e1Msg = iter->second; - auto compIter = e1Msg.mutable_components()->find(compKey.first); + auto compIter = e1Msg.mutable_components()->find(compPtr->TypeId()); ASSERT_TRUE(compIter != e1Msg.mutable_components()->end()); msgs::SerializedComponent &e1c1Msg = compIter->second; @@ -1960,7 +1960,7 @@ TEST_P(EntityComponentManagerFixture, ChangedStateComponents) EXPECT_EQ(0, changedStateMsg.entities_size()); // remove component - manager.RemoveComponent(e1, StringComponent::typeId); + EXPECT_TRUE(manager.RemoveComponent(e1, StringComponent::typeId)); changedStateMsg = manager.ChangedState(); EXPECT_EQ(1, changedStateMsg.entities_size()); @@ -2187,14 +2187,16 @@ TEST_P(EntityComponentManagerFixture, SetChanged) // Add components to each entity auto c1 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, c1); auto c2 = manager.CreateComponent(e2, IntComponent(456)); + ASSERT_NE(nullptr, c2); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(999, 888)); EXPECT_EQ(ComponentState::NoChange, manager.ComponentState(e1, 888)); @@ -2204,12 +2206,12 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_FALSE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); // Mark as changed - manager.SetChanged(e1, c1.first, ComponentState::PeriodicChange); + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); // check that only e1 c1 is serialized into a message msgs::SerializedStateMap stateMsg; @@ -2228,7 +2230,7 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_EQ(123, std::stoi(e1c1Msg.component())); } - manager.SetChanged(e2, c2.first, ComponentState::OneTimeChange); + manager.SetChanged(e2, c2->TypeId(), ComponentState::OneTimeChange); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); // Expect a single component type to be marked as PeriodicChange @@ -2236,23 +2238,23 @@ TEST_P(EntityComponentManagerFixture, SetChanged) EXPECT_EQ(IntComponent().TypeId(), *manager.ComponentTypesWithPeriodicChanges().begin()); EXPECT_EQ(ComponentState::PeriodicChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); EXPECT_EQ(ComponentState::OneTimeChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); // Remove components - EXPECT_TRUE(manager.RemoveComponent(e1, c1.first)); + EXPECT_TRUE(manager.RemoveComponent(e1, c1->TypeId())); EXPECT_TRUE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(0u, manager.ComponentTypesWithPeriodicChanges().size()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e1, c1.first)); + manager.ComponentState(e1, c1->TypeId())); - EXPECT_TRUE(manager.RemoveComponent(e2, c2.first)); + EXPECT_TRUE(manager.RemoveComponent(e2, c2->TypeId())); EXPECT_FALSE(manager.HasOneTimeComponentChanges()); EXPECT_EQ(ComponentState::NoChange, - manager.ComponentState(e2, c2.first)); + manager.ComponentState(e2, c2->TypeId())); } ////////////////////////////////////////////////// @@ -2280,20 +2282,23 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgAfterRemoveComponent) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectations; - expectations.insert(std::make_pair(e1c0.first, false)); - expectations.insert(std::make_pair(e1c1.first, true)); - expectations.insert(std::make_pair(e1c2.first, true)); + expectations.insert(std::make_pair(e1c0->TypeId(), false)); + expectations.insert(std::make_pair(e1c1->TypeId(), true)); + expectations.insert(std::make_pair(e1c2->TypeId(), true)); - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedStateMap stateMsg; @@ -2345,20 +2350,23 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMsgAfterRemoveComponent) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectations; - expectations.insert(std::make_pair(e1c0.first, false)); - expectations.insert(std::make_pair(e1c1.first, true)); - expectations.insert(std::make_pair(e1c2.first, true)); + expectations.insert(std::make_pair(e1c0->TypeId(), false)); + expectations.insert(std::make_pair(e1c1->TypeId(), true)); + expectations.insert(std::make_pair(e1c2->TypeId(), true)); - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedState stateMsg; @@ -2406,13 +2414,17 @@ TEST_P(EntityComponentManagerFixture, SerializedStateMapMsgCompsRemovedOnly) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); - manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c0); + auto e1c1 = manager.CreateComponent(e1, + DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); manager.RunSetAllComponentsUnchanged(); - manager.RemoveComponent(e1, e1c0); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c0->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // Serialize into a message msgs::SerializedStateMap stateMsg; manager.State(stateMsg); @@ -2446,19 +2458,22 @@ TEST_P(EntityComponentManagerFixture, SetRemovedComponentsMsgTypesFilter) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("foo")); + ASSERT_NE(nullptr, e1c2); manager.RunSetAllComponentsUnchanged(); - manager.RemoveComponent(e1, e1c0); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c0->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); // 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}; + std::unordered_set types{e1c0->TypeId(), e1c1->TypeId()}; manager.State(stateMsg, entitySet, types, false); // Check message @@ -2473,7 +2488,7 @@ TEST_P(EntityComponentManagerFixture, SetRemovedComponentsMsgTypesFilter) // Only component in message should be e1c2 const auto &c0 = compIter->second; EXPECT_EQ(c0.remove(), true); - EXPECT_EQ(c0.type(), e1c0.first); + EXPECT_EQ(c0.type(), e1c0->TypeId()); } } @@ -2487,17 +2502,20 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) Entity e1 = manager.CreateEntity(); auto e1c0 = manager.CreateComponent(e1, IntComponent(123)); + ASSERT_NE(nullptr, e1c0); auto e1c1 = manager.CreateComponent(e1, DoubleComponent(0.0)); + ASSERT_NE(nullptr, e1c1); auto e1c2 = manager.CreateComponent(e1, StringComponent("int")); + ASSERT_NE(nullptr, e1c2); // We use this map because the order in which components are iterated // through depends on the (undetermined) order of unordered multimaps std::map expectationsBeforeRemoving; - expectationsBeforeRemoving.insert(std::make_pair(e1c0.first, false)); - expectationsBeforeRemoving.insert(std::make_pair(e1c1.first, false)); - expectationsBeforeRemoving.insert(std::make_pair(e1c2.first, false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c0->TypeId(), false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c1->TypeId(), false)); + expectationsBeforeRemoving.insert(std::make_pair(e1c2->TypeId(), false)); // Serialize server ECM into a message msgs::SerializedStateMap stateMsg; @@ -2530,13 +2548,13 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } std::map expectationsAfterRemoving; - expectationsAfterRemoving.insert(std::make_pair(e1c0.first, false)); - expectationsAfterRemoving.insert(std::make_pair(e1c1.first, true)); - expectationsAfterRemoving.insert(std::make_pair(e1c2.first, true)); + expectationsAfterRemoving.insert(std::make_pair(e1c0->TypeId(), false)); + expectationsAfterRemoving.insert(std::make_pair(e1c1->TypeId(), true)); + expectationsAfterRemoving.insert(std::make_pair(e1c2->TypeId(), true)); // Remove components and synchronize again - manager.RemoveComponent(e1, e1c1); - manager.RemoveComponent(e1, e1c2); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c1->TypeId())); + EXPECT_TRUE(manager.RemoveComponent(e1, e1c2->TypeId())); msgs::SerializedStateMap newStateMsg; manager.State(newStateMsg); @@ -2574,6 +2592,356 @@ TEST_P(EntityComponentManagerFixture, RemovedComponentsSyncBetweenServerAndGUI) } } +/// \brief Helper function for comparing the same type of component across two +/// different entities +/// \param[in] _ecm The entity component manager +/// \param[in] _entity1 The first entity +/// \param[in] _entity2 The second entity +/// \param[in] _equal Whether the component's data between _entity1 and +/// _entity2 should be equal (true) or not (false) +/// \tparam ComponentTypeT Component type +template +static void CompareEntityComponents(const EntityComponentManager &_ecm, + const Entity _entity1, const Entity _entity2, bool _equal) +{ + auto comp1 = _ecm.Component(_entity1); + ASSERT_NE(nullptr, comp1); + + auto comp2 = _ecm.Component(_entity2); + ASSERT_NE(nullptr, comp2); + + if (_equal) + EXPECT_EQ(*comp1, *comp2); + else + EXPECT_NE(*comp1, *comp2); +} + +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, CloneEntities) +{ + // testing entity cloning with the following entity structure: + // - topLevelEntity + // - childEntity1 + // - grandChildEntity1 (canonical link for childEntity1) + // - childEntity2 (canonical link for topLevelEntity) + + const auto allowRename = true; + const auto noAllowRename = false; + + Entity topLevelEntity = manager.CreateEntity(); + manager.CreateComponent(topLevelEntity, components::Name("topLevelEntity")); + manager.CreateComponent(topLevelEntity, IntComponent(123)); + manager.CreateComponent(topLevelEntity, StringComponent("string0")); + + Entity childEntity1 = manager.CreateEntity(); + manager.CreateComponent(childEntity1, components::Name("childEntity1")); + manager.CreateComponent(childEntity1, + components::ParentEntity(topLevelEntity)); + manager.CreateComponent(childEntity1, IntComponent(456)); + manager.CreateComponent(childEntity1, StringComponent("string1")); + + Entity grandChildEntity1 = manager.CreateEntity(); + manager.CreateComponent(grandChildEntity1, + components::Name("grandChildEntity1")); + manager.CreateComponent(grandChildEntity1, + components::ParentEntity(childEntity1)); + + Entity childEntity2 = manager.CreateEntity(); + manager.CreateComponent(childEntity2, components::Name("childEntity2")); + manager.CreateComponent(childEntity2, + components::ParentEntity(topLevelEntity)); + manager.CreateComponent(childEntity2, IntComponent(789)); + manager.CreateComponent(childEntity2, StringComponent("string2")); + + manager.CreateComponent(topLevelEntity, + components::ModelCanonicalLink(childEntity2)); + manager.CreateComponent(childEntity1, + components::ModelCanonicalLink(grandChildEntity1)); + manager.CreateComponent(childEntity2, components::CanonicalLink()); + manager.CreateComponent(grandChildEntity1, components::CanonicalLink()); + + EXPECT_EQ(4u, manager.EntityCount()); + + std::unordered_set clonedEntities; + + auto validateTopLevelClone = + [&](const Entity _clonedEntity) + { + EXPECT_NE(kNullEntity, _clonedEntity); + EXPECT_NE(_clonedEntity, topLevelEntity); + EXPECT_EQ(manager.ComponentTypes(_clonedEntity), + manager.ComponentTypes(topLevelEntity)); + EXPECT_FALSE(manager.EntityHasComponentType(_clonedEntity, + components::ParentEntity::typeId)); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, false); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, true); + CompareEntityComponents(manager, topLevelEntity, + _clonedEntity, true); + CompareEntityComponents(manager, + topLevelEntity, _clonedEntity, false); + }; + + // clone the topLevelEntity + auto clonedTopLevelEntity = + manager.Clone(topLevelEntity, kNullEntity, "", allowRename); + EXPECT_EQ(8u, manager.EntityCount()); + clonedEntities.insert(clonedTopLevelEntity); + validateTopLevelClone(clonedTopLevelEntity); + + auto validateChildClone = + [&](const Entity _clonedChild, const Entity _originalChild) + { + EXPECT_NE(kNullEntity, _clonedChild); + EXPECT_NE(_clonedChild, _originalChild); + EXPECT_EQ(manager.ComponentTypes(_clonedChild), + manager.ComponentTypes(_originalChild)); + auto parentComp = + manager.Component(_clonedChild); + ASSERT_NE(nullptr, parentComp); + EXPECT_EQ(clonedTopLevelEntity, parentComp->Data()); + CompareEntityComponents(manager, _clonedChild, + _originalChild, false); + CompareEntityComponents(manager, _clonedChild, + _originalChild, true); + CompareEntityComponents(manager, _clonedChild, + _originalChild, true); + }; + + auto validateGrandChildClone = + [&](const Entity _clonedEntity, bool _sameParent) + { + EXPECT_NE(kNullEntity, _clonedEntity); + EXPECT_EQ(manager.ComponentTypes(_clonedEntity), + manager.ComponentTypes(grandChildEntity1)); + CompareEntityComponents(manager, _clonedEntity, + grandChildEntity1, false); + CompareEntityComponents(manager, + _clonedEntity, grandChildEntity1, _sameParent); + EXPECT_TRUE(manager.EntitiesByComponents( + components::ParentEntity(_clonedEntity)).empty()); + }; + + // Verify that all child entities were properly cloned + auto clonedChildEntities = manager.EntitiesByComponents( + components::ParentEntity(clonedTopLevelEntity)); + EXPECT_EQ(2u, clonedChildEntities.size()); + for (const auto &child : clonedChildEntities) + { + clonedEntities.insert(child); + + auto clonedGrandChildren = manager.EntitiesByComponents( + components::ParentEntity(child)); + + auto comparedToOriginalChild = false; + auto intComp = manager.Component(child); + ASSERT_NE(nullptr, intComp); + if (intComp->Data() == 456) + { + validateChildClone(child, childEntity1); + CompareEntityComponents(manager, child, + childEntity1, false); + comparedToOriginalChild = true; + + ASSERT_EQ(1u, clonedGrandChildren.size()); + clonedEntities.insert(clonedGrandChildren[0]); + validateGrandChildClone(clonedGrandChildren[0], false); + auto parentComp = + manager.Component(clonedGrandChildren[0]); + ASSERT_NE(nullptr, parentComp); + EXPECT_EQ(child, parentComp->Data()); + EXPECT_NE(nullptr, manager.Component( + clonedGrandChildren[0])); + } + else if (intComp->Data() == 789) + { + validateChildClone(child, childEntity2); + EXPECT_NE(nullptr, manager.Component(child)); + comparedToOriginalChild = true; + + EXPECT_TRUE(clonedGrandChildren.empty()); + } + + EXPECT_TRUE(comparedToOriginalChild); + } + + // clone a child entity + auto grandChildParentComp = + manager.Component(grandChildEntity1); + ASSERT_NE(nullptr, grandChildParentComp); + auto clonedGrandChildEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), "", allowRename); + EXPECT_EQ(9u, manager.EntityCount()); + clonedEntities.insert(clonedGrandChildEntity); + validateGrandChildClone(clonedGrandChildEntity, true); + + // Try cloning an entity with a name that already exists, but allow renaming. + // This should succeed and generate a cloned entity with a unique name. + const auto existingName = "grandChildEntity1"; + EXPECT_NE(kNullEntity, + manager.EntityByComponents(components::Name(existingName))); + auto renamedClonedEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), existingName, allowRename); + EXPECT_EQ(10u, manager.EntityCount()); + clonedEntities.insert(clonedGrandChildEntity); + validateGrandChildClone(renamedClonedEntity, true); + + // Try cloning an entity with a name that already exists, without allowing + // renaming. This should fail since entities should have unique names. + auto failedClonedEntity = manager.Clone(grandChildEntity1, + grandChildParentComp->Data(), existingName, noAllowRename); + EXPECT_EQ(10u, manager.EntityCount()); + EXPECT_EQ(kNullEntity, failedClonedEntity); + + // create a joint with a parent and child link + const std::string parentLinkEntityName = "parentLinkEntity"; + const std::string childLinkEntityName = "childLinkEntity"; + Entity parentLinkEntity = manager.CreateEntity(); + manager.CreateComponent(parentLinkEntity, + components::Name(parentLinkEntityName)); + manager.CreateComponent(parentLinkEntity, components::CanonicalLink()); + Entity jointEntity = manager.CreateEntity(); + manager.CreateComponent(jointEntity, + components::ParentEntity(parentLinkEntity)); + manager.CreateComponent(jointEntity, components::Name("jointEntity")); + manager.CreateComponent(jointEntity, components::Joint()); + manager.CreateComponent(jointEntity, + components::ParentLinkName(parentLinkEntityName)); + manager.CreateComponent(jointEntity, + components::ChildLinkName(childLinkEntityName)); + Entity childLinkEntity = manager.CreateEntity(); + manager.CreateComponent(childLinkEntity, + components::ParentEntity(jointEntity)); + manager.CreateComponent(childLinkEntity, + components::Name(childLinkEntityName)); + manager.CreateComponent(childLinkEntity, components::Link()); + EXPECT_EQ(13u, manager.EntityCount()); + + // clone a joint that has a parent and child link. + auto clonedParentLinkEntity = manager.Clone(parentLinkEntity, kNullEntity, + "", true); + ASSERT_NE(kNullEntity, clonedParentLinkEntity); + EXPECT_EQ(16u, manager.EntityCount()); + clonedEntities.insert(clonedParentLinkEntity); + auto clonedJoints = manager.EntitiesByComponents( + components::ParentEntity(clonedParentLinkEntity)); + ASSERT_EQ(1u, clonedJoints.size()); + clonedEntities.insert(clonedJoints[0]); + auto clonedChildLinks = manager.EntitiesByComponents( + components::ParentEntity(clonedJoints[0])); + ASSERT_EQ(1u, clonedChildLinks.size()); + clonedEntities.insert(clonedChildLinks[0]); + + // The cloned joint should have the cloned parent/child link names attached to + // it, not the original parent/child link names + auto clonedJointParentLinkName = + manager.Component(clonedJoints[0]); + ASSERT_NE(nullptr, clonedJointParentLinkName); + EXPECT_NE(clonedJointParentLinkName->Data(), parentLinkEntityName); + auto clonedJointChildLinkName = + manager.Component(clonedJoints[0]); + ASSERT_NE(nullptr, clonedJointChildLinkName); + EXPECT_NE(clonedJointChildLinkName->Data(), childLinkEntityName); + auto clonedParentLinkName = + manager.Component(clonedParentLinkEntity); + ASSERT_NE(nullptr, clonedParentLinkName); + EXPECT_EQ(clonedParentLinkName->Data(), clonedJointParentLinkName->Data()); + auto clonedChildLinkName = + manager.Component(clonedChildLinks[0]); + ASSERT_NE(nullptr, clonedChildLinkName); + EXPECT_EQ(clonedJointChildLinkName->Data(), clonedChildLinkName->Data()); + + // make sure that the name given to each cloned entity is unique + EXPECT_EQ(8u, clonedEntities.size()); + for (const auto &entity : clonedEntities) + { + auto nameComp = manager.Component(entity); + ASSERT_NE(nullptr, nameComp); + EXPECT_EQ(1u, manager.EntitiesByComponents(*nameComp).size()); + } + + // try to clone an entity that does not exist + EXPECT_EQ(kNullEntity, manager.Clone(kNullEntity, topLevelEntity, "", + allowRename)); + EXPECT_EQ(16u, manager.EntityCount()); +} + +///////////////////////////////////////////////// +// Check that some widely used deprecated APIs still work +TEST_P(EntityComponentManagerFixture, Deprecated) +{ + IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + + // Fail to create component for inexistent entity + EXPECT_EQ(nullptr, manager.CreateComponent(789, + IntComponent(123))); + + // Create some entities + auto eInt = manager.CreateEntity(); + auto eDouble = manager.CreateEntity(); + auto eIntDouble = manager.CreateEntity(); + EXPECT_EQ(3u, manager.EntityCount()); + + // Add components and keep their unique ComponentKeys + EXPECT_NE(nullptr, manager.CreateComponent(eInt, + IntComponent(123))); + ComponentKey cIntEInt = {IntComponent::typeId, eInt}; + + EXPECT_NE(nullptr, manager.CreateComponent(eDouble, + DoubleComponent(0.123))); + ComponentKey cDoubleEDouble = {DoubleComponent::typeId, eDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + IntComponent(456))); + ComponentKey cIntEIntDouble = {IntComponent::typeId, eIntDouble}; + + EXPECT_NE(nullptr, manager.CreateComponent(eIntDouble, + DoubleComponent(0.456))); + ComponentKey cDoubleEIntDouble = {DoubleComponent::typeId, eIntDouble}; + + // Check entities have the components + EXPECT_TRUE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_EQ(1u, manager.ComponentTypes(eInt).size()); + EXPECT_EQ(IntComponent::typeId, *manager.ComponentTypes(eInt).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eDouble).size()); + EXPECT_EQ(DoubleComponent::typeId, *manager.ComponentTypes(eDouble).begin()); + + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(2u, manager.ComponentTypes(eIntDouble).size()); + auto types = manager.ComponentTypes(eIntDouble); + EXPECT_NE(types.end(), types.find(IntComponent::typeId)); + EXPECT_NE(types.end(), types.find(DoubleComponent::typeId)); + + // Remove component by key + EXPECT_TRUE(manager.RemoveComponent(eInt, cIntEInt)); + EXPECT_FALSE(manager.EntityHasComponent(eInt, cIntEInt)); + EXPECT_TRUE(manager.ComponentTypes(eInt).empty()); + + // Remove component by type id + auto typeDouble = DoubleComponent::typeId; + + EXPECT_TRUE(manager.RemoveComponent(eDouble, typeDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eDouble, cDoubleEDouble)); + EXPECT_TRUE(manager.ComponentTypes(eDouble).empty()); + + // Remove component by type + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_TRUE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(1u, manager.ComponentTypes(eIntDouble).size()); + + EXPECT_TRUE(manager.RemoveComponent(eIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cIntEIntDouble)); + EXPECT_FALSE(manager.EntityHasComponent(eIntDouble, cDoubleEIntDouble)); + EXPECT_EQ(0u, manager.ComponentTypes(eIntDouble).size()); + + IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, PinnedEntity) { @@ -2632,6 +3000,102 @@ TEST_P(EntityComponentManagerFixture, PinnedEntity) EXPECT_EQ(0u, manager.EntityCount()); } +////////////////////////////////////////////////// +/// \brief Test using msgs::SerializedStateMap and msgs::SerializedState +/// to update existing component data between multiple ECMs +TEST_P(EntityComponentManagerFixture, StateMsgUpdateComponent) +{ + // create 2 ECMs: one will be modified directly, and the other should be + // updated to match the first via msgs::SerializedStateMap + EntityComponentManager originalECMStateMap; + EntityComponentManager otherECMStateMap; + + // create an entity and component + auto entity = originalECMStateMap.CreateEntity(); + originalECMStateMap.CreateComponent(entity, components::IntComponent(1)); + + int foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); + + // update the other ECM to have the new entity and component + msgs::SerializedStateMap stateMapMsg; + originalECMStateMap.State(stateMapMsg); + otherECMStateMap.SetState(stateMapMsg); + foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(1, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // modify a component and then share the update with the other ECM + stateMapMsg.Clear(); + originalECMStateMap.SetComponentData(entity, 2); + originalECMStateMap.State(stateMapMsg); + otherECMStateMap.SetState(stateMapMsg); + foundEntities = 0; + otherECMStateMap.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(2, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + // Run the same test as above, but this time, use a msgs::SerializedState + // instead of a msgs::SerializedStateMap + EntityComponentManager originalECMState; + EntityComponentManager otherECMState; + + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *) + { + foundEntities++; + return true; + }); + EXPECT_EQ(0, foundEntities); + + entity = originalECMState.CreateEntity(); + originalECMState.CreateComponent(entity, components::IntComponent(1)); + + auto stateMsg = originalECMState.State(); + otherECMState.SetState(stateMsg); + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(1, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); + + stateMsg.Clear(); + originalECMState.SetComponentData(entity, 2); + stateMsg = originalECMState.State(); + otherECMState.SetState(stateMsg); + foundEntities = 0; + otherECMState.Each( + [&](const Entity &, const components::IntComponent *_intComp) + { + foundEntities++; + EXPECT_EQ(2, _intComp->Data()); + return true; + }); + EXPECT_EQ(1, foundEntities); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(EntityComponentManagerRepeat, diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 1b4d2f39d2e..81fa242484a 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -25,6 +25,7 @@ #include #include +#include #include #include "ignition/gazebo/Events.hh" @@ -50,8 +51,10 @@ #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RenderEngineGuiPlugin.hh" +#include "ignition/gazebo/components/RenderEngineServerHeadless.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Wind.hh" #include "ignition/gazebo/components/World.hh" @@ -147,6 +150,10 @@ void LevelManager::ReadLevelPerformerInfo() components::RenderEngineServerPlugin( this->runner->serverConfig.RenderEngineServer())); + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::RenderEngineServerHeadless( + this->runner->serverConfig.HeadlessRendering())); + this->runner->entityCompMgr.CreateComponent(this->worldEntity, components::RenderEngineGuiPlugin( this->runner->serverConfig.RenderEngineGui())); @@ -180,13 +187,21 @@ void LevelManager::ReadLevelPerformerInfo() components::Atmosphere(*this->runner->sdfWorld->Atmosphere())); } + // spherical coordinates + if (this->runner->sdfWorld->SphericalCoordinates()) + { + this->runner->entityCompMgr.CreateComponent(this->worldEntity, + components::SphericalCoordinates( + *this->runner->sdfWorld->SphericalCoordinates())); + } + // TODO(anyone) This should probably go somewhere else as it is a global // constant. const std::string kPluginName{"ignition::gazebo"}; sdf::ElementPtr pluginElem; // Get the ignition::gazebo plugin element - for (auto plugin = worldElem->GetElement("plugin"); plugin; + for (auto plugin = worldElem->FindElement("plugin"); plugin; plugin = plugin->GetNextElement("plugin")) { if (plugin->Get("name") == kPluginName) diff --git a/src/ModelCommandAPI_TEST.cc b/src/ModelCommandAPI_TEST.cc index efb62254114..81beeb6b321 100644 --- a/src/ModelCommandAPI_TEST.cc +++ b/src/ModelCommandAPI_TEST.cc @@ -89,8 +89,9 @@ TEST(ModelCommandAPI, Commands) { ignition::gazebo::ServerConfig serverConfig; // Using an static model to avoid any movements in the simulation. - serverConfig.SetSdfFile(std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/static_diff_drive_vehicle.sdf"); + serverConfig.SetSdfFile( + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "static_diff_drive_vehicle.sdf")); ignition::gazebo::Server server(serverConfig); // Run at least one iteration before continuing to guarantee correctly set up. @@ -120,64 +121,64 @@ TEST(ModelCommandAPI, Commands) "Model: [8]\n" " - Name: vehicle_blue\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 2.000000 | 0.325000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 2.000000 0.325000]\n" + " [0.000000 0.000000 0.000000]\n" " - Link [9]\n" " - Name: chassis\n" " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [1.143950]\n" + " - Mass (kg): 1.143950\n" " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Inertial Matrix (kg.m^2):\n" - " [0.126164 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.416519 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.481014]\n" + " [0.126164 0.000000 0.000000]\n" + " [0.000000 0.416519 0.000000]\n" + " [0.000000 0.000000 0.481014]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [-0.151427 | 0.000000 | 0.175000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [-0.151427 0.000000 0.175000]\n" + " [0.000000 0.000000 0.000000]\n" " - Link [12]\n" " - Name: left_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [2.000000]\n" + " - Mass (kg): 2.000000\n" " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Inertial Matrix (kg.m^2):\n" - " [0.145833 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.145833 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.125000]\n" + " [0.145833 0.000000 0.000000]\n" + " [0.000000 0.145833 0.000000]\n" + " [0.000000 0.000000 0.125000]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.554283 | 0.625029 | -0.025000]\n" - " [-1.570700 | 0.000000 | 0.000000]\n" + " [0.554283 0.625029 -0.025000]\n" + " [-1.570700 0.000000 0.000000]\n" " - Link [15]\n" " - Name: right_wheel\n" " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [2.000000]\n" + " - Mass (kg): 2.000000\n" " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Inertial Matrix (kg.m^2):\n" - " [0.145833 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.145833 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.125000]\n" + " [0.145833 0.000000 0.000000]\n" + " [0.000000 0.145833 0.000000]\n" + " [0.000000 0.000000 0.125000]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.554282 | -0.625029 | -0.025000]\n" - " [-1.570700 | 0.000000 | 0.000000]\n" + " [0.554282 -0.625029 -0.025000]\n" + " [-1.570700 0.000000 0.000000]\n" " - Link [18]\n" " - Name: caster\n" " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [1.000000]\n" + " - Mass (kg): 1.000000\n" " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Inertial Matrix (kg.m^2):\n" - " [0.100000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.100000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.100000]\n" + " [0.100000 0.000000 0.000000]\n" + " [0.000000 0.100000 0.000000]\n" + " [0.000000 0.000000 0.100000]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [-0.957138 | 0.000000 | -0.125000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [-0.957138 0.000000 -0.125000]\n" + " [0.000000 0.000000 0.000000]\n" " - Joint [21]\n" " - Name: left_wheel_joint\n" " - Parent: vehicle_blue [8]\n" @@ -185,10 +186,10 @@ TEST(ModelCommandAPI, Commands) " - Parent Link: chassis [9]\n" " - Child Link: left_wheel [12]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Axis unit vector [ XYZ ]:\n" - " [0 | 0 | 1]\n" + " [0 0 1]\n" " - Joint [22]\n" " - Name: right_wheel_joint\n" " - Parent: vehicle_blue [8]\n" @@ -196,10 +197,10 @@ TEST(ModelCommandAPI, Commands) " - Parent Link: chassis [9]\n" " - Child Link: right_wheel [15]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" " - Axis unit vector [ XYZ ]:\n" - " [0 | 0 | 1]\n" + " [0 0 1]\n" " - Joint [23]\n" " - Name: caster_wheel\n" " - Parent: vehicle_blue [8]\n" @@ -207,8 +208,8 @@ TEST(ModelCommandAPI, Commands) " - Parent Link: chassis [9]\n" " - Child Link: caster [18]\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -222,8 +223,8 @@ TEST(ModelCommandAPI, Commands) "Model: [8]\n" " - Name: vehicle_blue\n" " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 2.000000 | 0.325000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + " [0.000000 2.000000 0.325000]\n" + " [0.000000 0.000000 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -235,62 +236,62 @@ TEST(ModelCommandAPI, Commands) ReplaceNegativeZeroValues(output); const std::string expectedOutput = "\nRequesting state for world [diff_drive]...\n\n" - " - Link [9]\n" - " - Name: chassis\n" - " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [1.143950]\n" - " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg.m^2):\n" - " [0.126164 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.416519 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.481014]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [-0.151427 | 0.000000 | 0.175000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Link [12]\n" - " - Name: left_wheel\n" - " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [2.000000]\n" - " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg.m^2):\n" - " [0.145833 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.145833 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.554283 | 0.625029 | -0.025000]\n" - " [-1.570700 | 0.000000 | 0.000000]\n" - " - Link [15]\n" - " - Name: right_wheel\n" - " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [2.000000]\n" - " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg.m^2):\n" - " [0.145833 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.145833 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.125000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.554282 | -0.625029 | -0.025000]\n" - " [-1.570700 | 0.000000 | 0.000000]\n" - " - Link [18]\n" - " - Name: caster\n" - " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [1.000000]\n" - " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg.m^2):\n" - " [0.100000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.100000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [-0.957138 | 0.000000 | -0.125000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + "- Link [9]\n" + " - Name: chassis\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): 1.143950\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" + " [0.126164 0.000000 0.000000]\n" + " [0.000000 0.416519 0.000000]\n" + " [0.000000 0.000000 0.481014]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [-0.151427 0.000000 0.175000]\n" + " [0.000000 0.000000 0.000000]\n" + "- Link [12]\n" + " - Name: left_wheel\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): 2.000000\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" + " [0.145833 0.000000 0.000000]\n" + " [0.000000 0.145833 0.000000]\n" + " [0.000000 0.000000 0.125000]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.554283 0.625029 -0.025000]\n" + " [-1.570700 0.000000 0.000000]\n" + "- Link [15]\n" + " - Name: right_wheel\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): 2.000000\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" + " [0.145833 0.000000 0.000000]\n" + " [0.000000 0.145833 0.000000]\n" + " [0.000000 0.000000 0.125000]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.554282 -0.625029 -0.025000]\n" + " [-1.570700 0.000000 0.000000]\n" + "- Link [18]\n" + " - Name: caster\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): 1.000000\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" + " [0.100000 0.000000 0.000000]\n" + " [0.000000 0.100000 0.000000]\n" + " [0.000000 0.000000 0.100000]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [-0.957138 0.000000 -0.125000]\n" + " [0.000000 0.000000 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -302,20 +303,20 @@ TEST(ModelCommandAPI, Commands) ReplaceNegativeZeroValues(output); const std::string expectedOutput = "\nRequesting state for world [diff_drive]...\n\n" - " - Link [18]\n" - " - Name: caster\n" - " - Parent: vehicle_blue [8]\n" - " - Mass (kg): [1.000000]\n" - " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Inertial Matrix (kg.m^2):\n" - " [0.100000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.100000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.100000]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [-0.957138 | 0.000000 | -0.125000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + "- Link [18]\n" + " - Name: caster\n" + " - Parent: vehicle_blue [8]\n" + " - Mass (kg): 1.000000\n" + " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Inertial Matrix (kg.m^2):\n" + " [0.100000 0.000000 0.000000]\n" + " [0.000000 0.100000 0.000000]\n" + " [0.000000 0.000000 0.100000]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [-0.957138 0.000000 -0.125000]\n" + " [0.000000 0.000000 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -327,37 +328,37 @@ TEST(ModelCommandAPI, Commands) ReplaceNegativeZeroValues(output); const std::string expectedOutput = "\nRequesting state for world [diff_drive]...\n\n" - " - Joint [21]\n" - " - Name: left_wheel_joint\n" - " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: chassis [9]\n" - " - Child Link: left_wheel [12]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]:\n" - " [0 | 0 | 1]\n" - " - Joint [22]\n" - " - Name: right_wheel_joint\n" - " - Parent: vehicle_blue [8]\n" - " - Type: revolute\n" - " - Parent Link: chassis [9]\n" - " - Child Link: right_wheel [15]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " - Axis unit vector [ XYZ ]:\n" - " [0 | 0 | 1]\n" - " - Joint [23]\n" - " - Name: caster_wheel\n" - " - Parent: vehicle_blue [8]\n" - " - Type: ball\n" - " - Parent Link: chassis [9]\n" - " - Child Link: caster [18]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + "- Joint [21]\n" + " - Name: left_wheel_joint\n" + " - Parent: vehicle_blue [8]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: left_wheel [12]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 0 1]\n" + "- Joint [22]\n" + " - Name: right_wheel_joint\n" + " - Parent: vehicle_blue [8]\n" + " - Type: revolute\n" + " - Parent Link: chassis [9]\n" + " - Child Link: right_wheel [15]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Axis unit vector [ XYZ ]:\n" + " [0 0 1]\n" + "- Joint [23]\n" + " - Name: caster_wheel\n" + " - Parent: vehicle_blue [8]\n" + " - Type: ball\n" + " - Parent Link: chassis [9]\n" + " - Child Link: caster [18]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n"; EXPECT_EQ(expectedOutput, output); } @@ -369,19 +370,174 @@ TEST(ModelCommandAPI, Commands) ReplaceNegativeZeroValues(output); const std::string expectedOutput = "\nRequesting state for world [diff_drive]...\n\n" - " - Joint [23]\n" - " - Name: caster_wheel\n" - " - Parent: vehicle_blue [8]\n" - " - Type: ball\n" - " - Parent Link: chassis [9]\n" - " - Child Link: caster [18]\n" - " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" - " [0.000000 | 0.000000 | 0.000000]\n" - " [0.000000 | 0.000000 | 0.000000]\n"; + "- Joint [23]\n" + " - Name: caster_wheel\n" + " - Parent: vehicle_blue [8]\n" + " - Type: ball\n" + " - Parent Link: chassis [9]\n" + " - Child Link: caster [18]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n"; + EXPECT_EQ(expectedOutput, output); + } +} + +///////////////////////////////////////////////// +// Tests `ign model -s` command with an altimeter. +TEST(ModelCommandAPI, AltimeterSensor) +{ + ignition::gazebo::ServerConfig serverConfig; + // Using an static model to avoid any movements in the simulation. + serverConfig.SetSdfFile( + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "altimeter_with_pose.sdf")); + + ignition::gazebo::Server server(serverConfig); + // Run at least one iteration before continuing to guarantee correctly set up. + ASSERT_TRUE(server.Run(true, 5, false)); + // Run without blocking. + server.Run(false, 0, false); + + // Tested command: ign model -m altimeter_mode -l link -s altimeter_sensor + { + const std::string cmd = kIgnModelCommand + + "-m altimeter_model -l link -s altimeter_sensor"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [altimeter_sensor]...\n\n" + "- Sensor [12]\n" + " - Name: altimeter_sensor\n" + " - Parent: altimeter_model [8]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.100000 0.200000 0.300000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Vertical position noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n" + " - Vertical velocity noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n"; EXPECT_EQ(expectedOutput, output); } } +///////////////////////////////////////////////// +// Tests `ign model -s` command with a magnetometer. +TEST(ModelCommandAPI, MagnetometerSensor) +{ + ignition::gazebo::ServerConfig serverConfig; + // Using an static model to avoid any movements in the simulation. + serverConfig.SetSdfFile( + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "magnetometer.sdf")); + + ignition::gazebo::Server server(serverConfig); + // Run at least one iteration before continuing to guarantee correctly set up. + ASSERT_TRUE(server.Run(true, 5, false)); + // Run without blocking. + server.Run(false, 0, false); + + // Tested command: ign model -m altimeter_mode -l link -s altimeter_sensor + { + const std::string cmd = kIgnModelCommand + + "-m magnetometer_model -l link -s magnetometer_sensor"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [magnetometer_sensor]...\n\n" + "- Sensor [12]\n" + " - Name: magnetometer_sensor\n" + " - Parent: magnetometer_model [8]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n"; + EXPECT_EQ(expectedOutput, output); + } +} + +///////////////////////////////////////////////// +// Tests `ign model -s` command with an rgbd camera. +TEST(ModelCommandAPI, RgbdCameraSensor) +{ + ignition::gazebo::ServerConfig serverConfig; + // Using an static model to avoid any movements in the simulation. + serverConfig.SetSdfFile( + ignition::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "rgbd_camera_sensor.sdf")); + + ignition::gazebo::Server server(serverConfig); + // Run at least one iteration before continuing to guarantee correctly set up. + ASSERT_TRUE(server.Run(true, 5, false)); + // Run without blocking. + server.Run(false, 0, false); + + // Tested command: ign model -m altimeter_mode -l link -s altimeter_sensor + { + const std::string cmd = kIgnModelCommand + + "-m rgbd_camera -l rgbd_camera_link -s rgbd_camera"; + std::string output = customExecStr(cmd); + ReplaceNegativeZeroValues(output); + const std::string expectedOutput = + "\nRequesting state for world [rgbd_camera_sensor]...\n\n" + "- Sensor [16]\n" + " - Name: rgbd_camera\n" + " - Parent: rgbd_camera [12]\n" + " - Pose [ XYZ (m) ] [ RPY (rad) ]:\n" + " [0.000000 0.000000 0.000000]\n" + " [0.000000 0.000000 0.000000]\n" + " - Horizontal field of view (rad): 1.05\n" + " - Image width (px): 256\n" + " - Image height (px): 256\n" + " - Near clip (m): 0.1\n" + " - Far clip (m): 10\n" + " - Pixel format: RGB_INT8\n" + " - Save frames: 0\n" + " - Save frames path: \n" + " - Image noise:\n" + " - Mean: 0\n" + " - Bias mean: 0\n" + " - Standard deviation: 0\n" + " - Bias standard deviation: 0\n" + " - Precision: 0\n" + " - Dynamic bias standard deviation: 0\n" + " - Dynamic bias correlation time: 0\n" + " - Distortion K1: 0\n" + " - Distortion K2: 0\n" + " - Distortion K3: 0\n" + " - Distortion P1: 0\n" + " - Distortion P2: 0\n" + " - Distortion center: 0.5 0.5\n" + " - Lens type: stereographic\n" + " - Lens scale to horizontal field of view (rad): 1\n" + " - Lens C1: 1\n" + " - Lens C2: 1\n" + " - Lens C3: 0\n" + " - Lens focal length (m): 1\n" + " - Lens function: tan\n" + " - Lens cutoff angle (rad): 1.5708\n" + " - Lens texture size: 256\n" + " - Lens intrinsics Fx: 277\n" + " - Lens intrinsics Fy: 277\n" + " - Lens intrinsics Cx: 160\n" + " - Lens intrinsics Cy: 120\n" + " - Lens intrinsics skew: 1\n" + " - Visibility mask: 4294967295\n"; + EXPECT_EQ(expectedOutput, output); + } +} + ////////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/src/Primitives.cc b/src/Primitives.cc new file mode 100644 index 00000000000..9e34eac618d --- /dev/null +++ b/src/Primitives.cc @@ -0,0 +1,364 @@ +/* + * 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 "ignition/gazebo/Primitives.hh" + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +constexpr const char * kBoxSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.16666 + 0 + 0 + 0.16666 + 0 + 0.16666 + + 1.0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + +///////////////////////////////////////////////// +constexpr const char * kSphereSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + 0.5 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + +///////////////////////////////////////////////// +constexpr const char * kCylinderSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.1458 + 0 + 0 + 0.1458 + 0 + 0.125 + + 1.0 + + + + + 0.5 + 1.0 + + + + + + + 0.5 + 1.0 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + +///////////////////////////////////////////////// +constexpr const char * kCapsuleSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.074154 + 0 + 0 + 0.074154 + 0 + 0.018769 + + 1.0 + + + + + 0.2 + 0.6 + + + + + + + 0.2 + 0.6 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + +///////////////////////////////////////////////// +constexpr const char *kEllipsoidSdf = R"( + + + 0 0 0.5 0 0 0 + + + + 0.068 + 0 + 0 + 0.058 + 0 + 0.026 + + 1.0 + + + + + 0.2 0.3 0.5 + + + + + + + 0.2 0.3 0.5 + + + + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 + + + + + +)"; + +///////////////////////////////////////////////// +constexpr const char *kDirectionalSdf = R"( + + + 0 0 2 0 0 0 + true + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + 0 0 -1 + + +)"; + +///////////////////////////////////////////////// +constexpr const char *kPointSdf = R"( + + + 0 0 2 0 0 0 + false + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + 4 + 0.2 + 0.5 + 0.01 + + + +)"; + +///////////////////////////////////////////////// +constexpr const char *kSpotSdf = R"( + + + 0 0 2 0 0 0 + true + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + + 4 + 0.2 + 0.5 + 0.01 + + 0 0 -1 + + 0.1 + 0.5 + 0.8 + + + +)"; + +///////////////////////////////////////////////// +std::string ignition::gazebo::getPrimitiveShape(const PrimitiveShape &_type) +{ + switch(_type) + { + case PrimitiveShape::kBox: + return kBoxSdf; + case PrimitiveShape::kSphere: + return kSphereSdf; + case PrimitiveShape::kCylinder: + return kCylinderSdf; + case PrimitiveShape::kCapsule: + return kCapsuleSdf; + case PrimitiveShape::kEllipsoid: + return kEllipsoidSdf; + default: + return ""; + } +} + +///////////////////////////////////////////////// +std::string ignition::gazebo::getPrimitiveLight(const PrimitiveLight &_type) +{ + switch(_type) + { + case PrimitiveLight::kDirectional: + return kDirectionalSdf; + case PrimitiveLight::kPoint: + return kPointSdf; + case PrimitiveLight::kSpot: + return kSpotSdf; + default: + return ""; + } +} + +///////////////////////////////////////////////// +std::string ignition::gazebo::getPrimitive(const std::string &_typeName) +{ + std::string type = common::lowercase(_typeName); + + if (type == "box") + return getPrimitiveShape(PrimitiveShape::kBox); + else if (type == "sphere") + return getPrimitiveShape(PrimitiveShape::kSphere); + else if (type == "cylinder") + return getPrimitiveShape(PrimitiveShape::kCylinder); + else if (type == "capsule") + return getPrimitiveShape(PrimitiveShape::kCapsule); + else if (type == "ellipsoid") + return getPrimitiveShape(PrimitiveShape::kEllipsoid); + else if (type == "point") + return getPrimitiveLight(PrimitiveLight::kPoint); + else if (type == "directional") + return getPrimitiveLight(PrimitiveLight::kDirectional); + else if (type == "spot") + return getPrimitiveLight(PrimitiveLight::kSpot); + + ignwarn << "Invalid model string " << type << "\n"; + ignwarn << "The valid options are:\n"; + ignwarn << " - box\n"; + ignwarn << " - sphere\n"; + ignwarn << " - cylinder\n"; + ignwarn << " - capsule\n"; + ignwarn << " - ellipsoid\n"; + ignwarn << " - point\n"; + ignwarn << " - directional\n"; + ignwarn << " - spot\n"; + return ""; +} diff --git a/src/Primitives_TEST.cc b/src/Primitives_TEST.cc new file mode 100644 index 00000000000..b6635e61448 --- /dev/null +++ b/src/Primitives_TEST.cc @@ -0,0 +1,95 @@ +/* + * 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 + +using PrimitiveShape = ignition::gazebo::PrimitiveShape; +using PrimitiveLight = ignition::gazebo::PrimitiveLight; + +///////////////////////////////////////////////// +TEST(Primitives, shapes) +{ + auto primitives = { + PrimitiveShape::kBox, + PrimitiveShape::kSphere, + PrimitiveShape::kCylinder, + PrimitiveShape::kCapsule, + PrimitiveShape::kEllipsoid + }; + + for (auto prim : primitives) + { + auto sdfString = ignition::gazebo::getPrimitiveShape(prim); + ASSERT_FALSE(sdfString.empty()); + + /// Verify that string contains valid SDF + sdf::Root root; + auto errors = root.LoadSdfString(sdfString); + EXPECT_TRUE(errors.empty()) << sdfString; + } +} + +///////////////////////////////////////////////// +TEST(Primitives, lights) +{ + auto primitives = { + PrimitiveLight::kDirectional, + PrimitiveLight::kPoint, + PrimitiveLight::kSpot, + }; + + for (auto prim : primitives) + { + auto sdfString = ignition::gazebo::getPrimitiveLight(prim); + ASSERT_FALSE(sdfString.empty()); + + /// Verify that string contains valid SDF + sdf::Root root; + auto errors = root.LoadSdfString(sdfString); + EXPECT_TRUE(errors.empty()) << sdfString; + } +} + +///////////////////////////////////////////////// +TEST(Primitives, invalid) +{ + auto sdfString = ignition::gazebo::getPrimitive("foobar"); + ASSERT_TRUE(sdfString.empty()); +} + +///////////////////////////////////////////////// +TEST(Primitives, strings) +{ + auto primitives = { + "box", "sphere", "cylinder", "capsule", "ellipsoid", + "point", "directional", "spot" + }; + + for (auto prim : primitives) + { + auto sdfString = ignition::gazebo::getPrimitive(prim); + ASSERT_FALSE(sdfString.empty()); + + /// Verify that string contains valid SDF + sdf::Root root; + auto errors = root.LoadSdfString(sdfString); + EXPECT_TRUE(errors.empty()) << sdfString; + } +} diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index 40b1be0ae2e..11db16cbb57 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -33,7 +33,9 @@ #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/CustomSensor.hh" #include "ignition/gazebo/components/DepthCamera.hh" +#include "ignition/gazebo/components/ForceTorque.hh" #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/GpuLidar.hh" #include "ignition/gazebo/components/Gravity.hh" @@ -62,9 +64,11 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/ThreadPitch.hh" @@ -236,6 +240,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::World *_world) components::Atmosphere(*_world->Atmosphere())); } + // spherical coordinates + if (_world->SphericalCoordinates()) + { + this->dataPtr->ecm->CreateComponent(worldEntity, + components::SphericalCoordinates(*_world->SphericalCoordinates())); + } + // Models for (uint64_t modelIndex = 0; modelIndex < _world->ModelCount(); ++modelIndex) @@ -438,7 +449,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Model *_model, << canonicalLinkPair.second << "\n"; } } - else + else if(!isStatic) { ignerr << "Could not resolve the canonical link for " << _model->Name() << "\n"; @@ -589,6 +600,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint) this->dataPtr->ecm->CreateComponent(jointEntity, components::JointType(_joint->Type())); + // Sensors + for (uint64_t sensorIndex = 0; sensorIndex < _joint->SensorCount(); + ++sensorIndex) + { + auto sensor = _joint->SensorByIndex(sensorIndex); + auto sensorEntity = this->CreateEntities(sensor); + + this->SetParent(sensorEntity, jointEntity); + } + if (_joint->Axis(0)) { auto resolvedAxis = ResolveJointAxis(*_joint->Axis(0)); @@ -782,7 +803,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) } else if (_sensor->Type() == sdf::SensorType::LIDAR) { - // \todo(anyone) Implement CPU-base lidar + // \todo(anyone) Implement CPU-based lidar // this->dataPtr->ecm->CreateComponent(sensorEntity, // components::Lidar(*_sensor)); ignwarn << "Sensor type LIDAR not supported yet. Try using" @@ -803,6 +824,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::ThermalCamera(*_sensor)); } + else if (_sensor->Type() == sdf::SensorType::SEGMENTATION_CAMERA) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::SegmentationCamera(*_sensor)); + } else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE) { this->dataPtr->ecm->CreateComponent(sensorEntity, @@ -836,6 +862,11 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::LinearAcceleration(math::Vector3d::Zero)); } + else if (_sensor->Type() == sdf::SensorType::FORCE_TORQUE) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::ForceTorque(*_sensor)); + } else if (_sensor->Type() == sdf::SensorType::LOGICAL_CAMERA) { auto elem = _sensor->Element(); @@ -865,6 +896,12 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) // We will let the contact system create the necessary components for // physics to populate. } + else if (_sensor->Type() == sdf::SensorType::CUSTOM) + { + auto elem = _sensor->Element(); + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::CustomSensor(*_sensor)); + } else { ignwarn << "Sensor type [" << static_cast(_sensor->Type()) diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 85fa6918fbc..e2ca63bfc1b 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -411,10 +411,13 @@ namespace sdf_generator auto poseElem = _elem->GetElement("pose"); // Remove all attributes of poseElem - sdf::ParamPtr relativeTo = poseElem->GetAttribute("relative_to"); - if (nullptr != relativeTo) + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) { - relativeTo->Reset(); + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } } poseElem->Set(poseComp->Data()); @@ -505,10 +508,13 @@ namespace sdf_generator auto poseElem = _elem->GetElement("pose"); // Remove all attributes of poseElem - sdf::ParamPtr relativeTo = poseElem->GetAttribute("relative_to"); - if (nullptr != relativeTo) + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) { - relativeTo->Reset(); + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } } poseElem->Set(poseComp->Data()); return true; diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index a2c030d9a01..aace52588f2 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -49,17 +49,22 @@ using namespace gazebo; ///////////////////////////////////////////////// /// \breif Checks if elemA is a subset of elemB -static bool isSubset(const sdf::ElementPtr &_elemA, +static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, const sdf::ElementPtr &_elemB) { if (_elemA->GetName() != _elemB->GetName()) { - return false; + return testing::AssertionFailure() + << "Mismatch in element name: '" << _elemA->GetName() << "' vs '" + << _elemB->GetName() << "'"; } if (_elemA->GetAttributeCount() != _elemB->GetAttributeCount()) { - return false; + return testing::AssertionFailure() + << "Mismatch in attribute count for " << _elemA->GetName() << ": " + << _elemA->GetAttributeCount() << " vs " + << _elemB->GetAttributeCount(); } // Compare attributes @@ -69,11 +74,17 @@ static bool isSubset(const sdf::ElementPtr &_elemA, sdf::ParamPtr attrB = _elemB->GetAttribute(attrA->GetKey()); if (attrA->GetTypeName() != attrB->GetTypeName()) { - return false; + return testing::AssertionFailure() + << "Mismatch in attribute type for " << _elemA->GetName() << "/[@" + << attrA->GetKey() << "]: '" << attrA->GetTypeName() << "' vs '" + << attrB->GetTypeName() << "'"; } if (attrA->GetAsString() != attrB->GetAsString()) { - return false; + return testing::AssertionFailure() + << "Mismatch in attribute as string for " << _elemA->GetName() + << "/[@" << attrA->GetKey() << "]: '" << attrA->GetAsString() + << "' vs '" << attrB->GetAsString() << "'"; } } // Compare values @@ -83,11 +94,35 @@ static bool isSubset(const sdf::ElementPtr &_elemA, { sdf::ParamPtr valB = _elemB->GetValue(); if (nullptr == valB) - return false; + { + return testing::AssertionFailure() + << "Value of " << _elemB->GetName() << " null"; + } if (valA->GetTypeName() != valB->GetTypeName()) - return false; - if (valA->GetAsString() != valB->GetAsString()) - return false; + { + return testing::AssertionFailure() + << "Mismatch in value type for " << _elemA->GetName() << ": '" + << valA->GetTypeName() << "' vs '" << valB->GetTypeName() << "'"; + } + if (valA->GetTypeName() == "pose") + { + math::Pose3d poseA, poseB; + valA->Get(poseA); + valA->Get(poseB); + + if (poseA != poseB) + { + return testing::AssertionFailure() + << "Mismatch in value as Pose: '" << poseA << "' vs '" << poseB + << "'"; + } + } + else if (valA->GetAsString() != valB->GetAsString()) + { + return testing::AssertionFailure() + << "Mismatch in value as string: '" << valA->GetAsString() + << "' vs '" << valB->GetAsString() << "'"; + } } } @@ -112,10 +147,14 @@ static bool isSubset(const sdf::ElementPtr &_elemA, } if (!result) - return false; + { + return testing::AssertionFailure() + << "No matching child element in element B for child element '" + << childElemA->GetName() << "' in element A"; + } } - return true; + return testing::AssertionSuccess(); } ///////////////////////////////////////////////// @@ -124,15 +163,15 @@ TEST(CompareElements, CompareWithDuplicateElements) const std::string m1Sdf = R"( - 0 0 0 0 0 0 0 + 0 0 0 0 0 0 )"; const std::string m1CompTestSdf = R"( - 0 0 0 0 0 0 0 - 0 0 0 0 0 0 0 + 0 0 0 0 0 0 + 0 0 0 0 0 0 )"; @@ -878,6 +917,76 @@ TEST_F(GenerateWorldFixture, ModelsInline) } } +///////////////////////////////////////////////// +TEST_F(GenerateWorldFixture, PoseWithAttributes) +{ + const auto includeUri = std::string("file://") + PROJECT_SOURCE_PATH + + "/test/worlds/models/relative_resource_uri"; + + std::string worldSdf = R"( + + + + + )" + includeUri + R"( + 1 2 3 90 0 0 + model1 + + + )" + includeUri + R"( + 1 2 3 0 0 0 1 + model2 + + + + )"; + + this->LoadWorldString(worldSdf); + Entity worldEntity = this->ecm.EntityByComponents(components::World()); + + auto testPoses = [](const std::string &_worldStr) + { + sdf::Root newRoot; + sdf::Errors errors = newRoot.LoadSdfString(_worldStr); + EXPECT_TRUE(errors.empty()) << errors; + auto newWorld = newRoot.WorldByIndex(0); + ASSERT_NE(nullptr, newWorld); + { + auto model = newWorld->ModelByIndex(0); + ASSERT_NE(nullptr, model); + // Check that the generated element has the new pose + EXPECT_EQ(math::Pose3d(1, 2, 3, IGN_PI_2, 0, 0), model->RawPose()); + } + { + auto model = newWorld->ModelByIndex(1); + ASSERT_NE(nullptr, model); + // Check that the generated element has the new pose + EXPECT_EQ(math::Pose3d(1, 2, 3, 0, 0, 0), model->RawPose()); + } + }; + + // Test with models included models expanded + { + this->sdfGenConfig.mutable_global_entity_gen_config() + ->mutable_expand_include_tags() + ->set_data(true); + + auto worldStr = sdf_generator::generateWorld( + this->ecm, worldEntity, this->includeUriMap, this->sdfGenConfig); + ASSERT_TRUE(worldStr.has_value()); + SCOPED_TRACE("Included models expanded"); + testPoses(*worldStr); + } + + // Test with models included models not expanded + { + auto worldStr = sdf_generator::generateWorld(this->ecm, worldEntity); + ASSERT_TRUE(worldStr.has_value()); + SCOPED_TRACE("Included models not expanded"); + testPoses(*worldStr); + } +} + ///////////////////////////////////////////////// /// Main int main(int _argc, char **_argv) diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 68da657f8bf..fb0d9fdace7 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -220,11 +220,11 @@ class ignition::gazebo::ServerConfigPrivate public: explicit ServerConfigPrivate( const std::unique_ptr &_cfg) : sdfFile(_cfg->sdfFile), + sdfString(_cfg->sdfString), updateRate(_cfg->updateRate), useLevels(_cfg->useLevels), useLogRecord(_cfg->useLogRecord), logRecordPath(_cfg->logRecordPath), - logIgnoreSdfPath(_cfg->logIgnoreSdfPath), logPlaybackPath(_cfg->logPlaybackPath), logRecordResources(_cfg->logRecordResources), logRecordCompressPath(_cfg->logRecordCompressPath), @@ -236,7 +236,8 @@ class ignition::gazebo::ServerConfigPrivate networkRole(_cfg->networkRole), networkSecondaries(_cfg->networkSecondaries), seed(_cfg->seed), - logRecordTopics(_cfg->logRecordTopics) { } + logRecordTopics(_cfg->logRecordTopics), + isHeadlessRendering(_cfg->isHeadlessRendering) { } // \brief The SDF file that the server should load public: std::string sdfFile = ""; @@ -256,10 +257,6 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Path to place recorded states public: std::string logRecordPath = ""; - /// TODO(anyone) Deprecate in public APIs in Ignition-D, remove in Ignition-E - /// \brief Whether log record path is specified from command line - public: bool logIgnoreSdfPath{false}; - /// \brief Path to recorded states to play back using logging system public: std::string logPlaybackPath = ""; @@ -301,6 +298,9 @@ class ignition::gazebo::ServerConfigPrivate /// \brief Topics to record. public: std::vector logRecordTopics; + + /// \brief is the headless mode active. + public: bool isHeadlessRendering{false}; }; ////////////////////////////////////////////////// @@ -441,18 +441,6 @@ void ServerConfig::SetLogRecordPath(const std::string &_recordPath) this->dataPtr->logRecordPath = _recordPath; } -///////////////////////////////////////////////// -bool ServerConfig::LogIgnoreSdfPath() const -{ - return this->dataPtr->logIgnoreSdfPath; -} - -///////////////////////////////////////////////// -void ServerConfig::SetLogIgnoreSdfPath(bool _ignore) -{ - this->dataPtr->logIgnoreSdfPath = _ignore; -} - ///////////////////////////////////////////////// const std::string ServerConfig::LogPlaybackPath() const { @@ -538,6 +526,18 @@ void ServerConfig::SetRenderEngineServer(const std::string &_renderEngineServer) this->dataPtr->renderEngineServer = _renderEngineServer; } +///////////////////////////////////////////////// +void ServerConfig::SetHeadlessRendering(const bool _headless) +{ + this->dataPtr->isHeadlessRendering = _headless; +} + +///////////////////////////////////////////////// +bool ServerConfig::HeadlessRendering() const +{ + return this->dataPtr->isHeadlessRendering; +} + ///////////////////////////////////////////////// const std::string &ServerConfig::RenderEngineGui() const { @@ -878,7 +878,7 @@ ignition::gazebo::loadPluginInfo(bool _isPlayback) std::string defaultConfigDir; ignition::common::env(IGN_HOMEDIR, defaultConfigDir); defaultConfigDir = ignition::common::joinPaths(defaultConfigDir, ".ignition", - "gazebo"); + "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR); auto defaultConfig = ignition::common::joinPaths(defaultConfigDir, configFilename); diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 844d997795d..1369cb47cfd 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -195,12 +195,14 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // TODO(louise) Combine both messages into one. this->node->Advertise("control", &SimulationRunner::OnWorldControl, this); + this->node->Advertise("control/state", &SimulationRunner::OnWorldControlState, + this); this->node->Advertise("playback/control", &SimulationRunner::OnPlaybackControl, this); ignmsg << "Serving world controls on [" << opts.NameSpace() - << "/control] and [" << opts.NameSpace() << "/playback/control]" - << std::endl; + << "/control], [" << opts.NameSpace() << "/control/state] and [" + << opts.NameSpace() << "/playback/control]" << std::endl; // Publish empty GUI messages for worlds that have no GUI in the beginning. // In the future, support modifying GUI from the server at runtime. @@ -410,6 +412,12 @@ void SimulationRunner::PublishStats() msg.set_paused(this->currentInfo.paused); + if (this->Stepping()) + { + auto headerData = msg.mutable_header()->add_data(); + headerData->set_key("step"); + } + // Publish the stats message. The stats message is throttled. this->statsPub.Publish(msg); @@ -572,6 +580,7 @@ void SimulationRunner::UpdateSystems() { IGN_PROFILE("PostUpdate"); + this->entityCompMgr.LockAddingEntitiesToViews(true); // If no systems implementing PostUpdate have been added, then // the barriers will be uninitialized, so guard against that condition. if (this->postUpdateStartBarrier && this->postUpdateStopBarrier) @@ -579,6 +588,7 @@ void SimulationRunner::UpdateSystems() this->postUpdateStartBarrier->Wait(); this->postUpdateStopBarrier->Wait(); } + this->entityCompMgr.LockAddingEntitiesToViews(false); } } @@ -659,7 +669,13 @@ bool SimulationRunner::Run(const uint64_t _iterations) if (!this->statsPub.Valid()) { transport::AdvertiseMessageOptions advertOpts; - advertOpts.SetMsgsPerSec(5); + // publish 10 world statistics msgs/second. A smaller number isn't used + // because the GUI listens to these msgs to receive confirmation that + // pause/play GUI requests have been processed by the server, so we want to + // make sure that GUI requests are acknowledged quickly (see + // https://github.com/ignitionrobotics/ign-gui/pull/306 and + // https://github.com/ignitionrobotics/ign-gazebo/pull/1163) + advertOpts.SetMsgsPerSec(10); this->statsPub = this->node->Advertise( "stats", advertOpts); } @@ -988,7 +1004,7 @@ void SimulationRunner::LoadLoggingPlugins(const ServerConfig &_config) void SimulationRunner::LoadPlugins(const Entity _entity, const sdf::ElementPtr &_sdf) { - sdf::ElementPtr pluginElem = _sdf->GetElement("plugin"); + sdf::ElementPtr pluginElem = _sdf->FindElement("plugin"); while (pluginElem) { auto filename = pluginElem->Get("filename"); @@ -996,8 +1012,7 @@ void SimulationRunner::LoadPlugins(const Entity _entity, // No error message for the 'else' case of the following 'if' statement // because SDF create a default element even if it's not // specified. An error message would result in spamming - // the console. \todo(nkoenig) Fix SDF should so that elements are not - // automatically added. + // the console. if (filename != "__default__" && name != "__default__") { this->LoadPlugin(_entity, filename, name, pluginElem); @@ -1077,6 +1092,18 @@ void SimulationRunner::SetPaused(const bool _paused) this->currentInfo.paused = _paused; } +///////////////////////////////////////////////// +void SimulationRunner::SetStepping(bool _stepping) +{ + this->stepping = _stepping; +} + +///////////////////////////////////////////////// +bool SimulationRunner::Stepping() const +{ + return this->stepping; +} + ///////////////////////////////////////////////// void SimulationRunner::SetRunToSimTime( const std::chrono::steady_clock::duration &_time) @@ -1095,36 +1122,54 @@ void SimulationRunner::SetRunToSimTime( ///////////////////////////////////////////////// bool SimulationRunner::OnWorldControl(const msgs::WorldControl &_req, msgs::Boolean &_res) +{ + msgs::WorldControlState req; + req.mutable_world_control()->CopyFrom(_req); + + return this->OnWorldControlState(req, _res); +} + +///////////////////////////////////////////////// +bool SimulationRunner::OnWorldControlState(const msgs::WorldControlState &_req, + msgs::Boolean &_res) { std::lock_guard lock(this->msgBufferMutex); + // update the server ECM if the request contains SerializedState information + if (_req.has_state()) + this->entityCompMgr.SetState(_req.state()); + // TODO(anyone) notify server systems of changes made to the ECM, if there + // were any? + WorldControl control; - control.pause = _req.pause(); + control.pause = _req.world_control().pause(); - if (_req.multi_step() != 0) - control.multiStep = _req.multi_step(); - else if (_req.step()) + if (_req.world_control().multi_step() != 0) + control.multiStep = _req.world_control().multi_step(); + else if (_req.world_control().step()) control.multiStep = 1; - if (_req.has_reset()) + if (_req.world_control().has_reset()) { - control.rewind = _req.reset().all() || _req.reset().time_only(); + control.rewind = _req.world_control().reset().all() || + _req.world_control().reset().time_only(); - if (_req.reset().model_only()) + if (_req.world_control().reset().model_only()) { ignwarn << "Model only reset is not supported." << std::endl; } } - if (_req.seed() != 0) + if (_req.world_control().seed() != 0) { ignwarn << "Changing seed is not supported." << std::endl; } - if (_req.has_run_to_sim_time()) + if (_req.world_control().has_run_to_sim_time()) { - control.runToSimTime = std::chrono::seconds(_req.run_to_sim_time().sec()) + - std::chrono::nanoseconds(_req.run_to_sim_time().nsec()); + control.runToSimTime = std::chrono::seconds( + _req.world_control().run_to_sim_time().sec()) + + std::chrono::nanoseconds(_req.world_control().run_to_sim_time().nsec()); } this->worldControls.push_back(control); @@ -1173,6 +1218,10 @@ void SimulationRunner::ProcessMessages() void SimulationRunner::ProcessWorldControl() { IGN_PROFILE("SimulationRunner::ProcessWorldControl"); + + // assume no stepping unless WorldControl msgs say otherwise + this->SetStepping(false); + for (const auto &control : this->worldControls) { // Play / pause @@ -1184,6 +1233,7 @@ void SimulationRunner::ProcessWorldControl() this->pendingSimIterations += control.multiStep; // Unpause so that stepping can occur. this->SetPaused(false); + this->SetStepping(true); } // Rewind / reset diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 36754d33887..9f17d59b6b3 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -38,6 +38,7 @@ #include #include #include +#include #include #include "ignition/gazebo/config.hh" @@ -291,6 +292,17 @@ namespace ignition /// \return True if the simulation runner is paused, false otherwise. public: bool Paused() const; + /// \brief Set if the simulation runner is stepping based on WorldControl + /// info + /// \param[in] _step True if stepping based on WorldControl info, false + /// otherwise + public: void SetStepping(bool _step); + + /// \brief Get if the simulation runner is stepping based on WorldControl + /// info + /// \return True if stepping based on WorldControl info, false otherwise + public: bool Stepping() const; + /// \brief Set the run to simulation time. /// \param[in] _time A simulation time in the future to run to and then /// pause. A negative number or a time less than the current simulation @@ -370,6 +382,16 @@ namespace ignition private: bool OnWorldControl(const msgs::WorldControl &_req, msgs::Boolean &_res); + /// \brief World control state service callback. This function stores the + /// the request which will then be processed by the ProcessMessages + /// function. + /// \param[in] _req Request from client, currently handling play / pause + /// and multistep. This also may contain SerializedState information. + /// \param[out] _res Response to client, true if successful. + /// \return True for success + private: bool OnWorldControlState(const msgs::WorldControlState &_req, + msgs::Boolean &_res); + /// \brief World control service callback. This function stores the /// the request which will then be processed by the ProcessMessages /// function. @@ -601,6 +623,10 @@ namespace ignition /// \brief True if Server::RunOnce triggered a blocking paused step private: bool blockingPausedStepPending{false}; + /// \brief Whether the simulation runner is currently stepping based on + /// WorldControl info (true) or not (false) + private: bool stepping{false}; + friend class LevelManager; }; } diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index cd2a864eb11..6176cfdc891 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1532,7 +1532,7 @@ TEST_P(SimulationRunnerTest, GuiInfo) auto plugin = res.plugin(0); EXPECT_EQ("3D View", plugin.name()); - EXPECT_EQ("GzScene3D", plugin.filename()); + EXPECT_EQ("MinimalScene", plugin.filename()); EXPECT_NE(plugin.innerxml().find(""), std::string::npos); EXPECT_NE(plugin.innerxml().find(""), std::string::npos); EXPECT_EQ(plugin.innerxml().find(""), std::string::npos); @@ -1568,6 +1568,52 @@ TEST_P(SimulationRunnerTest, GenerateWorldSdf) EXPECT_EQ(5u, world->ModelCount()); } +///////////////////////////////////////////////// +/// Helper function to recursively check for plugins with filename and name +/// attributes set to "__default__" +testing::AssertionResult checkForSpuriousPlugins(sdf::ElementPtr _elem) +{ + auto plugin = _elem->FindElement("plugin"); + if (nullptr != plugin && + plugin->Get("filename") == "__default__" && + plugin->Get("name") == "__default__") + { + return testing::AssertionFailure() << _elem->ToString(""); + } + for (auto child = _elem->GetFirstElement(); child; + child = child->GetNextElement()) + { + auto result = checkForSpuriousPlugins(child); + if (!result) + return result; + } + return testing::AssertionSuccess(); +} + +///////////////////////////////////////////////// +TEST_P(SimulationRunnerTest, GeneratedSdfHasNoSpuriousPlugins) +{ + // Load SDF file + sdf::Root root; + root.Load(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "shapes.sdf")); + + ASSERT_EQ(1u, root.WorldCount()); + + // Create simulation runner + auto systemLoader = std::make_shared(); + SimulationRunner runner(root.WorldByIndex(0), systemLoader); + + msgs::SdfGeneratorConfig req; + msgs::StringMsg genWorldSdf; + EXPECT_TRUE(runner.GenerateWorldSdf(req, genWorldSdf)); + EXPECT_FALSE(genWorldSdf.data().empty()); + + sdf::Root newRoot; + newRoot.LoadSdfString(genWorldSdf.data()); + EXPECT_TRUE(checkForSpuriousPlugins(newRoot.Element())); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, SimulationRunnerTest, diff --git a/src/Util.cc b/src/Util.cc index 9681512d858..ec6efd57dcc 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -47,6 +47,7 @@ #include "ignition/gazebo/components/ParticleEmitter.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -554,6 +555,30 @@ std::string validTopic(const std::vector &_topics) } return std::string(); } + +////////////////////////////////////////////////// +std::optional sphericalCoordinates(Entity _entity, + const EntityComponentManager &_ecm) +{ + auto sphericalCoordinatesComp = + _ecm.Component( + worldEntity(_entity, _ecm)); + if (nullptr == sphericalCoordinatesComp) + { + return std::nullopt; + } + + auto xyzPose = worldPose(_entity, _ecm); + + // lat / lon / elevation in rad / rad / m + auto rad = sphericalCoordinatesComp->Data().PositionTransform( + xyzPose.Pos(), + math::SphericalCoordinates::LOCAL2, + math::SphericalCoordinates::SPHERICAL); + + // Return degrees + return math::Vector3d(IGN_RTOD(rad.X()), IGN_RTOD(rad.Y()), rad.Z()); +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 62417e000c6..ac83bf2643c 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Collision.hh" @@ -446,7 +447,7 @@ TEST_F(UtilTest, AsFullPath) // Data string { - const std::string path{"data-string"}; + const std::string path{sdf::kSdfStringSource}; EXPECT_EQ(relativeUriUnix, asFullPath(relativeUriUnix, path)); EXPECT_EQ(relativeUriWindows, asFullPath(relativeUriWindows, path)); diff --git a/src/View.cc b/src/View.cc deleted file mode 100644 index 09ac442984b..00000000000 --- a/src/View.cc +++ /dev/null @@ -1,84 +0,0 @@ -/* - * 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 "ignition/gazebo/detail/View.hh" -#include "ignition/gazebo/EntityComponentManager.hh" - -using namespace ignition; -using namespace gazebo; -using namespace detail; - -////////////////////////////////////////////////// -void View::AddEntity(const Entity _entity, const bool _new) -{ - this->entities.insert(_entity); - if (_new) - { - this->newEntities.insert(_entity); - } -} - -////////////////////////////////////////////////// -void View::AddComponent(const Entity _entity, - const ComponentTypeId _typeId, - const ComponentId _componentId) -{ - this->components.insert( - std::make_pair(std::make_pair(_entity, _typeId), _componentId)); -} - -////////////////////////////////////////////////// -bool View::RemoveEntity(const Entity _entity, const ComponentTypeKey &_key) -{ - if (this->entities.find(_entity) == this->entities.end()) - return false; - - // Otherwise, remove the entity from the view - this->entities.erase(_entity); - this->newEntities.erase(_entity); - this->toRemoveEntities.erase(_entity); - - // Remove the entity from the components map - for (const ComponentTypeId &compTypeId : _key) - this->components.erase(std::make_pair(_entity, compTypeId)); - - return true; -} - -///////////////////////////////////////////////// -const components::BaseComponent *View::ComponentImplementation( - const Entity _entity, - ComponentTypeId _typeId, - const EntityComponentManager *_ecm) const -{ - return _ecm->ComponentImplementation( - {_typeId, this->components.at({_entity, _typeId})}); -} - -////////////////////////////////////////////////// -void View::ClearNewEntities() -{ - this->newEntities.clear(); -} - -////////////////////////////////////////////////// -bool View::AddEntityToRemoved(const Entity _entity) -{ - if (this->entities.find(_entity) == this->entities.end()) - return false; - this->toRemoveEntities.insert(_entity); - return true; -} diff --git a/src/World.cc b/src/World.cc index 333a0f430ca..2d1514f79c5 100644 --- a/src/World.cc +++ b/src/World.cc @@ -15,6 +15,7 @@ * */ +#include #include #include "ignition/gazebo/components/Actor.hh" @@ -25,6 +26,7 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/World.hh" @@ -91,6 +93,34 @@ std::optional World::Atmosphere( return _ecm.ComponentData(this->dataPtr->id); } +////////////////////////////////////////////////// +std::optional World::SphericalCoordinates( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +void World::SetSphericalCoordinates(EntityComponentManager &_ecm, + const math::SphericalCoordinates &_sphericalCoordinates) +{ + auto sphericalCoordinatesComp = + _ecm.Component(this->dataPtr->id); + if (!sphericalCoordinatesComp) + { + _ecm.CreateComponent(this->dataPtr->id, + components::SphericalCoordinates(_sphericalCoordinates)); + return; + } + + sphericalCoordinatesComp->SetData(_sphericalCoordinates, + [](const math::SphericalCoordinates &, + const math::SphericalCoordinates &){return false;}); + _ecm.SetChanged(this->dataPtr->id, + components::SphericalCoordinates::typeId, ComponentState::OneTimeChange); +} + ////////////////////////////////////////////////// std::optional World::Gravity( const EntityComponentManager &_ecm) const diff --git a/src/cmd/ModelCommandAPI.cc b/src/cmd/ModelCommandAPI.cc index 6c9b80eb060..f467668e9fd 100644 --- a/src/cmd/ModelCommandAPI.cc +++ b/src/cmd/ModelCommandAPI.cc @@ -21,23 +21,36 @@ #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 #include #include #include +#include +#include #include #include @@ -117,19 +130,329 @@ std::string entityInfo(const std::string &_name, ////////////////////////////////////////////////// /// \brief Get pose info in a standard way /// \param[in] _pose Pose to print -/// \param[in] _prefix Indentation prefix for every line +/// \param[in] _spaces Number of spaces to indent for every line /// \return Pose formatted in a standard way -std::string poseInfo(math::Pose3d _pose, const std::string &_prefix) +std::string poseInfo(math::Pose3d _pose, int _spaces) { return - _prefix + "[" + std::to_string(_pose.X()) + " | " - + std::to_string(_pose.Y()) + " | " + std::string(_spaces, ' ') + "[" + std::to_string(_pose.X()) + " " + + std::to_string(_pose.Y()) + " " + std::to_string(_pose.Z()) + "]\n" + - _prefix + "[" + std::to_string(_pose.Roll()) + " | " - + std::to_string(_pose.Pitch()) + " | " + std::string(_spaces, ' ') + "[" + std::to_string(_pose.Roll()) + " " + + std::to_string(_pose.Pitch()) + " " + std::to_string(_pose.Yaw()) + "]"; } +////////////////////////////////////////////////// +/// \brief Print pose info about an entity. +/// \param[in] _entity Entity to print pose information for. Nothing is +/// printed if the entity lack a pose component. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printPose(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + const auto poseComp = _ecm.Component(_entity); + if (poseComp) + { + std::cout << std::string(_spaces, ' ') + << "- Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), _spaces + 2) << std::endl; + } +} + +////////////////////////////////////////////////// +/// \brief Print noise information. +/// \param[in] _noise Noise to print. +/// \param[in] _spaces Number of spaces to indent for every line. +void printNoise(const sdf::Noise &_noise, int _spaces) +{ + std::cout << std::string(_spaces, ' ') << "- Mean: " << _noise.Mean() << "\n" + << std::string(_spaces, ' ') << "- Bias mean: " + << _noise.BiasMean() << "\n" + << std::string(_spaces, ' ') << "- Standard deviation: " + << _noise.StdDev() << "\n" + << std::string(_spaces, ' ') << "- Bias standard deviation: " + << _noise.BiasStdDev() << "\n" + << std::string(_spaces, ' ') << "- Precision: " + << _noise.Precision() << "\n" + << std::string(_spaces, ' ') << "- Dynamic bias standard deviation: " + << _noise.DynamicBiasStdDev() << "\n" + << std::string(_spaces, ' ') << "- Dynamic bias correlation time: " + << _noise.DynamicBiasCorrelationTime() << std::endl; +} + +////////////////////////////////////////////////// +/// \brief Print info about an altimeter sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an altimeter. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printAltimeter(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Altimeter *altimeter = sensor.AltimeterSensor(); + + std::cout << std::string(_spaces, ' ') << "- Vertical position noise:\n"; + printNoise(altimeter->VerticalPositionNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') << "- Vertical velocity noise:\n"; + printNoise(altimeter->VerticalVelocityNoise(), _spaces + 2); +} + +////////////////////////////////////////////////// +/// \brief Print info about an SDF camera. +/// \param[in] _camera The camera to output. +/// \param[in] _spaces Number of spaces to indent for every line. +void printCamera(const sdf::Camera *_camera, int _spaces) +{ + std::cout << std::string(_spaces, ' ') + << "- Horizontal field of view (rad): " << _camera->HorizontalFov() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Image width (px): " << _camera->ImageWidth() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Image height (px): " << _camera->ImageHeight() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Near clip (m): " << _camera->NearClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Far clip (m): " << _camera->FarClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Pixel format: " << _camera->PixelFormatStr() + << std::endl; + + if (_camera->HasDepthCamera()) + { + std::cout << std::string(_spaces, ' ') + << "- Depth near clip (m): " << _camera->DepthNearClip() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Depth far clip (m): " << _camera->DepthFarClip() + << std::endl; + } + + if (_camera->HasSegmentationType()) + { + std::cout << std::string(_spaces, ' ') + << "- Segmentation type: " << _camera->SegmentationType() + << std::endl; + } + + if (_camera->HasBoundingBoxType()) + { + std::cout << std::string(_spaces, ' ') + << "- Bounding box type: " << _camera->BoundingBoxType() + << std::endl; + } + + std::cout << std::string(_spaces, ' ') + << "- Save frames: " << _camera->SaveFrames() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Save frames path: " << _camera->SaveFramesPath() + << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Image noise:\n"; + printNoise(_camera->ImageNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Distortion K1: " << _camera->DistortionK1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion K2: " << _camera->DistortionK2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion K3: " << _camera->DistortionK3() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion P1: " << _camera->DistortionP1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion P2: " << _camera->DistortionP2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Distortion center: " << _camera->DistortionCenter() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens type: " << _camera->LensType() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens scale to horizontal field of view (rad): " + << _camera->LensScaleToHfov() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C1: " << _camera->LensC1() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C2: " << _camera->LensC2() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens C3: " << _camera->LensC3() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens focal length (m): " << _camera->LensFocalLength() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens function: " << _camera->LensFunction() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens cutoff angle (rad): " << _camera->LensCutoffAngle() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens texture size: " << _camera->LensEnvironmentTextureSize() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Fx: " << _camera->LensIntrinsicsFx() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Fy: " << _camera->LensIntrinsicsFy() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Cx: " << _camera->LensIntrinsicsCx() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics Cy: " << _camera->LensIntrinsicsCy() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Lens intrinsics skew: " << _camera->LensIntrinsicsSkew() + << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Visibility mask: " << _camera->VisibilityMask() + << std::endl; +} + +////////////////////////////////////////////////// +/// \brief Print info about an RGBD camera sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an RGBD camera. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printRgbdCamera(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Camera *camera = sensor.CameraSensor(); + + printCamera(camera, _spaces); +} + +////////////////////////////////////////////////// +/// \brief Print info about a camera sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not a camera. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printCamera(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Camera *camera = sensor.CameraSensor(); + + printCamera(camera, _spaces); +} + +////////////////////////////////////////////////// +/// \brief Print info about an imu sensor. +/// \param[in] _entity Entity to print information for. Nothing is +/// printed if the entity is not an IMU. +/// \param[in] _ecm The entity component manager. +/// \param[in] _spaces Number of spaces to indent for every line +void printImu(const uint64_t _entity, const EntityComponentManager &_ecm, + int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Imu *imu = sensor.ImuSensor(); + + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration X-axis noise:\n"; + printNoise(imu->LinearAccelerationXNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration Y-axis noise:\n"; + printNoise(imu->LinearAccelerationYNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Linear acceleration Z-axis noise:\n"; + printNoise(imu->LinearAccelerationZNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Angular velocity X-axis noise:\n"; + printNoise(imu->AngularVelocityXNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Angular velocity Y-axis noise:\n"; + printNoise(imu->AngularVelocityYNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') + << "- Angular velocity Z-axis noise:\n"; + printNoise(imu->AngularVelocityZNoise(), _spaces + 2); + + std::cout << std::string(_spaces, ' ') + << "- Gravity direction X [XYZ]: " + << imu->GravityDirX() << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Gravity direction X parent frame:" << imu->GravityDirXParentFrame() + << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Localization:" << imu->Localization() << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Custom RPY: " << imu->CustomRpy() << std::endl; + std::cout << std::string(_spaces, ' ') + << "- Custom RPY parent frame:" << imu->CustomRpyParentFrame() << std::endl; + + std::cout << std::string(_spaces, ' ') + << "- Orientation enabled:" << imu->OrientationEnabled() << std::endl; +} + +////////////////////////////////////////////////// +void printMagnetometer(const uint64_t _entity, + const EntityComponentManager &_ecm, int _spaces) +{ + // Get the type and return if the _entity does not have the correct + // component. + auto comp = _ecm.Component(_entity); + if (!comp) + return; + + const sdf::Sensor &sensor = comp->Data(); + const sdf::Magnetometer *mag = sensor.MagnetometerSensor(); + + std::cout << std::string(_spaces, ' ') << "- X-axis noise:\n"; + printNoise(mag->XNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') << "- Y-axis noise:\n"; + printNoise(mag->YNoise(), _spaces + 2); + std::cout << std::string(_spaces, ' ') << "- Z-axis noise:\n"; + printNoise(mag->ZNoise(), _spaces + 2); +} + ////////////////////////////////////////////////// // \brief Set the state of a ECM instance with a world snapshot. // \param _ecm ECM instance to be populated. @@ -189,9 +512,8 @@ void printModelInfo(const uint64_t _entity, if (poseComp && nameComp) { std::cout << "Model: [" << _entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + << " - Name: " << nameComp->Data() << std::endl; + printPose(_entity, _ecm, 2); } } @@ -202,7 +524,9 @@ void printModelInfo(const uint64_t _entity, // \param[in] _linkName Link to be printed, if empty, print all links. void printLinks(const uint64_t _modelEntity, const EntityComponentManager &_ecm, - const std::string &_linkName) + const std::string &_linkName, + const std::string &_sensorName, + int _spaces) { const auto links = _ecm.EntitiesByComponents( components::ParentEntity(_modelEntity), components::Link()); @@ -210,46 +534,79 @@ void printLinks(const uint64_t _modelEntity, { const auto nameComp = _ecm.Component(entity); + int spaces = _spaces; + if (_linkName.length() && _linkName != nameComp->Data()) continue; - std::cout << " - Link [" << entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << entityInfo(_modelEntity, _ecm) - << std::endl; - - const auto inertialComp = _ecm.Component(entity); - - if (inertialComp) + if (_sensorName.empty()) { - const auto inertialMatrix = inertialComp->Data().MassMatrix(); - const auto mass = inertialComp->Data().MassMatrix().Mass(); - - const std::string massInfo = "[" + std::to_string(mass) + "]"; - const std::string inertialInfo = - "\n [" + std::to_string(inertialMatrix.Ixx()) + " | " - + std::to_string(inertialMatrix.Ixy()) + " | " - + std::to_string(inertialMatrix.Ixz()) + "]\n" - " [" + std::to_string(inertialMatrix.Ixy()) + " | " - + std::to_string(inertialMatrix.Iyy()) + " | " - + std::to_string(inertialMatrix.Iyz()) + "]\n" - " [" + std::to_string(inertialMatrix.Ixz()) + " | " - + std::to_string(inertialMatrix.Iyz()) + " | " - + std::to_string(inertialMatrix.Izz()) + "]"; - std::cout << " - Mass (kg): " << massInfo << std::endl - << " - Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" - << std::endl - << poseInfo(inertialComp->Data().Pose(), " ") - << std::endl - << " - Inertial Matrix (kg.m^2):" - << inertialInfo << std::endl; + std::cout << std::string(spaces, ' ') + << "- Link [" << entity << "]" << std::endl + << std::string(spaces + 2, ' ') + << "- Name: " << nameComp->Data() << std::endl + << std::string(spaces + 2, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; + + const auto inertialComp = _ecm.Component(entity); + + if (inertialComp) + { + const auto inertialMatrix = inertialComp->Data().MassMatrix(); + const auto mass = inertialComp->Data().MassMatrix().Mass(); + + std::cout << std::string(spaces + 2, ' ') + << "- Mass (kg): " << std::to_string(mass) << std::endl + << std::string(spaces + 2, ' ') + << "- Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:" + << std::endl + << poseInfo(inertialComp->Data().Pose(), spaces + 4) + << std::endl + << std::string(spaces + 2, ' ') + << "- Inertial Matrix (kg.m^2):\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixx()) << " " + << std::to_string(inertialMatrix.Ixy()) << " " + << std::to_string(inertialMatrix.Ixz()) << "]\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixy()) << " " + << std::to_string(inertialMatrix.Iyy()) << " " + << std::to_string(inertialMatrix.Iyz()) << "]\n" + << std::string(spaces + 4, ' ') << "[" + << std::to_string(inertialMatrix.Ixz()) << " " + << std::to_string(inertialMatrix.Iyz()) << " " + << std::to_string(inertialMatrix.Izz()) << "]" + << std::endl; + } + + printPose(entity, _ecm, spaces + 2); + + spaces += 2; } - const auto poseComp = _ecm.Component(entity); - if (poseComp) + const auto sensors = _ecm.EntitiesByComponents( + components::ParentEntity(entity), components::Sensor()); + for (const auto &sensor : sensors) { - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + const auto sensorNameComp = _ecm.Component(sensor); + if (!_sensorName.empty() && _sensorName != sensorNameComp->Data()) + continue; + + std::cout << std::string(spaces, ' ') + << "- Sensor [" << sensor << "]\n"; + std::cout << std::string(spaces + 2, ' ') + << "- Name: " << sensorNameComp->Data() << "\n" + << std::string(spaces + 2, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) << std::endl; + printPose(sensor, _ecm, spaces + 2); + + // Run through all the sensor print statements. Each function will + // exit early if the the sensor is the wrong type. + printAltimeter(sensor, _ecm, spaces + 2); + printCamera(sensor, _ecm, spaces + 2); + printImu(sensor, _ecm, spaces + 2); + printRgbdCamera(sensor, _ecm, spaces + 2); } } } @@ -261,7 +618,8 @@ void printLinks(const uint64_t _modelEntity, // \param[in] _jointName Joint to be printed, if nullptr, print all joints. void printJoints(const uint64_t _modelEntity, const EntityComponentManager &_ecm, - const std::string &_jointName) + const std::string &_jointName, + int _spaces) { static const std::map jointTypes = { @@ -286,16 +644,22 @@ void printJoints(const uint64_t _modelEntity, if (_jointName.length() && _jointName != nameComp->Data()) continue; - std::cout << " - Joint [" << entity << "]" << std::endl - << " - Name: " << nameComp->Data() << std::endl - << " - Parent: " << entityInfo(_modelEntity, _ecm) - << std::endl; + int spaces = _spaces; + std::cout << std::string(_spaces, ' ') + << "- Joint [" << entity << "]" << std::endl; + spaces += 2; + + std::cout << std::string(spaces, ' ') + << "- Name: " << nameComp->Data() << std::endl + << std::string(spaces, ' ') + << "- Parent: " << entityInfo(_modelEntity, _ecm) + << std::endl; const auto jointTypeComp = _ecm.Component(entity); if (jointTypeComp) { - std::cout << " - Type: " << jointTypes.at(jointTypeComp->Data()) - << std::endl; + std::cout << std::string(spaces, ' ') + << "- Type: " << jointTypes.at(jointTypeComp->Data()) << std::endl; } const auto childLinkComp = @@ -305,26 +669,26 @@ void printJoints(const uint64_t _modelEntity, if (childLinkComp && parentLinkComp) { - std::cout << " - Parent Link: " + std::cout << std::string(spaces, ' ') << "- Parent Link: " << entityInfo(parentLinkComp->Data(), _ecm) << "\n" - << " - Child Link: " + << std::string(spaces, ' ') << "- Child Link: " << entityInfo(childLinkComp->Data(), _ecm) << "\n"; } const auto poseComp = _ecm.Component(entity); if (poseComp) { - std::cout << " - Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl - << poseInfo(poseComp->Data(), " ") << std::endl; + std::cout << std::string(spaces, ' ') + << "- Pose [ XYZ (m) ] [ RPY (rad) ]:" << std::endl + << poseInfo(poseComp->Data(), spaces + 2) << std::endl; } const auto axisComp = _ecm.Component(entity); if (axisComp) { - std::cout << " - Axis unit vector [ XYZ ]:\n" - " [" << axisComp->Data().Xyz().X() << " | " - << axisComp->Data().Xyz().Y() << " | " - << axisComp->Data().Xyz().Z() << "]\n"; + std::cout << std::string(spaces, ' ') << "- Axis unit vector [ XYZ ]:\n" + << std::string(spaces + 2, ' ') << "[" << axisComp->Data().Xyz() + << "]\n"; } } } @@ -366,7 +730,7 @@ extern "C" void cmdModelList() ////////////////////////////////////////////////// extern "C" void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, - const char *_jointName) + const char *_jointName, const char *_sensorName) { std::string linkName{""}; if (_linkName) @@ -374,8 +738,11 @@ extern "C" void cmdModelInfo( std::string jointName{""}; if (_jointName) jointName = _jointName; + std::string sensorName{""}; + if (_sensorName) + sensorName = _sensorName; bool printAll{false}; - if (!_pose && !_linkName && !_jointName) + if (!_pose && !_linkName && !_jointName && !_sensorName) printAll = true; if (!_modelName) @@ -395,15 +762,19 @@ extern "C" void cmdModelInfo( if (entity == kNullEntity) std::cout << "No model named <" << _modelName << "> was found" << std::endl; + int spaces = 0; // Get the pose of the model - if (printAll | _pose) + if (printAll || _pose) + { printModelInfo(entity, ecm); + spaces += 2; + } // Get the links information - if (printAll | (_linkName != nullptr)) - printLinks(entity, ecm, linkName); + if (printAll || _linkName != nullptr || _sensorName != nullptr) + printLinks(entity, ecm, linkName, sensorName, spaces); // Get the joints information - if (printAll | (_jointName != nullptr)) - printJoints(entity, ecm, jointName); + if (printAll || (_jointName != nullptr)) + printJoints(entity, ecm, jointName, spaces); } diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index 5cd2ed761ff..2ca7248c609 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -21,9 +21,11 @@ extern "C" void cmdModelList(); /// \brief External hook to dump model information. /// \param[in] _modelName Model name. /// \param[in] _pose --pose option. -/// \param[in] _link_name Link name. -/// \param[in] _joint_name Joint name. +/// \param[in] _linkName Link name. +/// \param[in] _jointName Joint name. +/// \param[in] _sensorName Sensor name. extern "C" void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, - const char *_jointName); + const char *_jointName, + const char *_sensorName); diff --git a/src/cmd/cmdgazebo.rb.in b/src/cmd/cmdgazebo.rb.in index 30f783c6eb5..dfb46fec7b1 100755 --- a/src/cmd/cmdgazebo.rb.in +++ b/src/cmd/cmdgazebo.rb.in @@ -73,6 +73,8 @@ COMMANDS = { 'gazebo' => " enable console logging to a console.log \n"\ " file in the specified path. \n"\ "\n"\ + " --headless-rendering Run rendering in headless mode \n"\ + "\n"\ " --record-resources Implicitly invokes --record, and records \n"\ " meshes and material files, in addition to \n"\ " states and console messages. \n"\ @@ -158,13 +160,7 @@ COMMANDS = { 'gazebo' => " locate system plugins. \n\n"\ " IGN_GAZEBO_SERVER_CONFIG_PATH Path to server configuration file. \n\n"\ " IGN_GUI_PLUGIN_PATH Colon separated paths used to locate GUI \n"\ - " plugins. \n\n"\ - " IGN_GAZEBO_NETWORK_ROLE Participant role used in a distributed \n"\ - " simulation environment. Role is one of [PRIMARY, SECONDARY]. This is \n"\ - " deprecated in ign-gazebo2. Please use --network-role instead. \n\n"\ - " IGN_GAZEBO_NETWORK_SECONDARIES Number of secondary participants \n"\ - " expected to join a distributed simulation environment. (Primary only) \n"\ - " This is deprecated in ign-gazebo2. Please use --network-role instead. \n" + " plugins. \n"\ } # @@ -223,8 +219,9 @@ class Cmd 'verbose' => '1', 'gui_config' => '', 'physics_engine' => '', - 'rendering_engine_gui' => '', - 'rendering_engine_server' => '' + 'render_engine_gui' => '', + 'render_engine_server' => '', + 'headless-rendering' => 0 } usage = COMMANDS[args[0]] @@ -291,6 +288,9 @@ class Cmd opts.on('--physics-engine [arg]', String) do |e| options['physics_engine'] = e end + opts.on('--headless-rendering') do + options['headless-rendering'] = 1 + end opts.on('--render-engine-gui [arg]', String) do |g| options['render_engine_gui'] = g end @@ -431,10 +431,10 @@ has properly set the DYLD_LIBRARY_PATH environment variables." const char *, int, int, const char *, int, int, int, const char *, const char *, const char *, const char *, const char *, - const char *)' + const char *, int)' # Import the runGui function - Importer.extern 'int runGui(const char *)' + Importer.extern 'int runGui(const char *, const char *)' # If playback is specified, and the user has not specified a # custom gui config, set the gui config to load the playback @@ -464,14 +464,15 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." options['log-overwrite'], options['log-compress'], options['playback'], options['physics_engine'], options['render_engine_server'], options['render_engine_gui'], - options['file'], options['record-topics'].join(':')) + options['file'], options['record-topics'].join(':'), + options['headless-rendering']) end guiPid = Process.fork do ENV['RMT_PORT'] = '1501' Process.setpgid(0, 0) Process.setproctitle('ign gazebo gui') - Importer.runGui(options['gui_config']) + Importer.runGui(options['gui_config'], options['render_engine_gui']) end Signal.trap("INT") { @@ -499,7 +500,8 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." options['log-overwrite'], options['log-compress'], options['playback'], options['physics_engine'], options['render_engine_server'], options['render_engine_gui'], - options['file'], options['record-topics'].join(':')) + options['file'], options['record-topics'].join(':'), + options['headless-rendering']) # Otherwise run the gui else options['gui'] if plugin.end_with? ".dylib" @@ -509,7 +511,7 @@ See https://github.com/ignitionrobotics/ign-gazebo/issues/44 for more info." end ENV['RMT_PORT'] = '1501' - Importer.runGui(options['gui_config']) + Importer.runGui(options['gui_config'], options['render_engine_gui']) end rescue puts "Library error: Problem running [#{options['command']}]() "\ diff --git a/src/cmd/cmdmodel.rb.in b/src/cmd/cmdmodel.rb.in index 8070bfec638..88e65a50f3d 100644 --- a/src/cmd/cmdmodel.rb.in +++ b/src/cmd/cmdmodel.rb.in @@ -55,6 +55,14 @@ COMMANDS = { 'model' => " caster link in the diff_drive world, run: \n"\ " ign model -m vehicle_blue -l caster \n"\ " \n"\ + " -s [--sensor] arg Select a sensor to show its properties. \n"\ + " If no arg is passed all sensors are printed\n"\ + " Requires the -m and -l options \n"\ + " \n"\ + " E.g. to get information about the \n"\ + " imu sensor in the sensors world, run: \n"\ + " ign model -m sensors_box -l link -s imu \n"\ + " \n"\ " -j [--joint] arg Select a joint to show its properties. \n"\ " If no arg is passed all joints are printed \n"\ " Requires the -m option \n"\ @@ -78,7 +86,8 @@ class Cmd options = { 'pose' => 0, 'link_name' => 0, - 'joint_name' => 0 + 'joint_name' => 0, + 'sensor_name' => 0 } usage = COMMANDS[args[0]] @@ -104,8 +113,14 @@ class Cmd options['link_name'] = l end end + opts.on('-s', '--sensor [arg]', String, + 'Request sensor information') do |s| + options['sensor_name'] = '' + if s + options['sensor_name'] = s + end + end opts.on('-j', '--joint [arg]', String, 'Request joint information') do |j| - # options['joint'] = 1 options['joint_name'] = '' if j options['joint_name'] = j @@ -166,9 +181,9 @@ class Cmd exit(0) elsif options.key?('model_name') Importer.extern 'void cmdModelInfo(const char *, int, const char *, - const char *)' + const char *, const char *)' Importer.cmdModelInfo(options['model_name'], options['pose'], - options['link_name'], options['joint_name']) + options['link_name'], options['joint_name'], options['sensor_name']) else puts 'Command error: I do not have an implementation for '\ "command [ign #{options['command']}]." diff --git a/src/cmd/ign.cc b/src/cmd/ign.cc index ad8f7df4b70..ac3ff8932d8 100644 --- a/src/cmd/ign.cc +++ b/src/cmd/ign.cc @@ -25,9 +25,9 @@ #include #include "ignition/gazebo/config.hh" -#include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/ServerConfig.hh" +#include "gui/GuiRunner.hh" #include "ign.hh" ////////////////////////////////////////////////// @@ -228,10 +228,14 @@ extern "C" int runGui(const char *_guiConfig) app.Engine()->addImportPath(IGN_GAZEBO_GUI_PLUGIN_INSTALL_DIR); // Set default config file for Gazebo - std::string defaultConfig; - ignition::common::env(IGN_HOMEDIR, defaultConfig); - defaultConfig = ignition::common::joinPaths(defaultConfig, ".ignition", - "gazebo", "gui.config"); + std::string defaultConfigDir; + ignition::common::env(IGN_HOMEDIR, defaultConfigDir); + defaultConfigDir = ignition::common::joinPaths(defaultConfig, ".ignition", + "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR); + + auto defaultConfig = ignition::common::joinPaths(defaultConfigDir, + "gui.config"); + app.SetDefaultConfigPath(defaultConfig); // Customize window @@ -403,6 +407,23 @@ extern "C" int runGui(const char *_guiConfig) { auto installedConfig = ignition::common::joinPaths( IGNITION_GAZEBO_GUI_CONFIG_PATH, "gui.config"); + + if (!ignition::common::createDirectories(defaultConfigDir)) + { + ignerr << "Failed to create directory [" << defaultConfigDir + << "]." << std::endl; + return -1; + } + + if (!ignition::common::exists(installedConfig)) + { + ignerr << "Failed to copy installed config [" << installedConfig + << "] to default config [" << defaultConfig << "]." + << "(file " << installedConfig << " doesn't exist)" + << std::endl; + return -1; + } + if (!ignition::common::copyFile(installedConfig, defaultConfig)) { ignerr << "Failed to copy installed config [" << installedConfig @@ -418,7 +439,7 @@ extern "C" int runGui(const char *_guiConfig) } } - // Also set ~/.ignition/gazebo/gui.config as the default path + // Also set ~/.ignition/gazebo/ver/gui.config as the default path if (!app.LoadConfig(defaultConfig)) { ignerr << "Failed to load config file[" << _guiConfig << "]." diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index 98957534399..ae0de7d3da0 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -1,14 +1,15 @@ set (gui_sources AboutDialogHandler.cc Gui.cc + GuiEvents.cc GuiFileHandler.cc GuiRunner.cc PathManager.cc - TmpIface.cc ) set (gtest_sources Gui_TEST.cc + GuiEvents_TEST.cc ) add_subdirectory(plugins) @@ -25,10 +26,6 @@ set(CMAKE_AUTORCC ON) # no need to add entries for Qt header files in `src/gui/`. qt5_wrap_cpp(gui_sources ${PROJECT_SOURCE_DIR}/include/ignition/gazebo/gui/GuiSystem.hh - # TODO (anyone) Remove when GuiRunner.hh is moved to `src` - ${PROJECT_SOURCE_DIR}/include/ignition/gazebo/gui/GuiRunner.hh - # TODO (anyone) Remove in v6 - ${PROJECT_SOURCE_DIR}/include/ignition/gazebo/gui/TmpIface.hh ) if (MSVC) diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index f121dec6a0a..7de934c158e 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -24,11 +24,11 @@ #include #include "ignition/gazebo/config.hh" -#include "ignition/gazebo/gui/GuiRunner.hh" - #include "ignition/gazebo/gui/Gui.hh" + #include "AboutDialogHandler.hh" #include "GuiFileHandler.hh" +#include "GuiRunner.hh" #include "PathManager.hh" namespace ignition @@ -39,11 +39,11 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace gui { - ////////////////////////////////////////////////// std::unique_ptr createGui( int &_argc, char **_argv, const char *_guiConfig, - const char *_defaultGuiConfig, bool _loadPluginsFromSdf) + const char *_defaultGuiConfig, bool _loadPluginsFromSdf, + const char *_renderEngine) { ignition::common::SignalHandler sigHandler; bool sigKilled = false; @@ -80,6 +80,10 @@ std::unique_ptr createGui( // Set default config file for Gazebo std::string defaultConfig; + + // Default config folder. + std::string defaultConfigFolder; + if (nullptr == _defaultGuiConfig) { // The playback flag (and not the gui-config flag) was @@ -89,8 +93,11 @@ std::unique_ptr createGui( defaultGuiConfigName = "playback_gui.config"; } ignition::common::env(IGN_HOMEDIR, defaultConfig); - defaultConfig = ignition::common::joinPaths(defaultConfig, ".ignition", - "gazebo", defaultGuiConfigName); + defaultConfigFolder = + ignition::common::joinPaths(defaultConfig, ".ignition", + "gazebo", IGNITION_GAZEBO_MAJOR_VERSION_STR); + defaultConfig = ignition::common::joinPaths(defaultConfigFolder, + defaultGuiConfigName); } else { @@ -101,6 +108,10 @@ std::unique_ptr createGui( // Customize window auto mainWin = app->findChild(); + if (_renderEngine != nullptr) + { + mainWin->SetRenderEngine(_renderEngine); + } auto win = mainWin->QuickWindow(); win->setProperty("title", "Gazebo"); @@ -169,20 +180,7 @@ std::unique_ptr createGui( // TODO(anyone) Most of ign-gazebo's transport API includes the world name, // which makes it complicated to mix configurations across worlds. // We could have a way to use world-agnostic topics like Gazebo-classic's ~ - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif auto runner = new ignition::gazebo::GuiRunner(worldsMsg.data(0)); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif ++runnerCount; runner->setParent(ignition::gui::App()); @@ -231,20 +229,7 @@ std::unique_ptr createGui( } // GUI runner - // Remove warning suppression in v6 -#ifndef _WIN32 -# pragma GCC diagnostic push -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#else -# pragma warning(push) -# pragma warning(disable: 4996) -#endif auto runner = new ignition::gazebo::GuiRunner(worldName); -#ifndef _WIN32 -# pragma GCC diagnostic pop -#else -# pragma warning(pop) -#endif runner->setParent(ignition::gui::App()); ++runnerCount; @@ -284,8 +269,27 @@ std::unique_ptr createGui( // the installed file there first. if (!ignition::common::exists(defaultConfig)) { + if (!ignition::common::exists(defaultConfigFolder)) + { + if (!ignition::common::createDirectories(defaultConfigFolder)) + { + ignerr << "Failed to create the default config folder [" + << defaultConfigFolder << "]\n"; + return nullptr; + } + } + auto installedConfig = ignition::common::joinPaths( IGNITION_GAZEBO_GUI_CONFIG_PATH, defaultGuiConfigName); + if (!ignition::common::exists(installedConfig)) + { + ignerr << "Failed to copy installed config [" << installedConfig + << "] to default config [" << defaultConfig << "]." + << "(file " << installedConfig << " doesn't exist)" + << std::endl; + return nullptr; + } + if (!ignition::common::copyFile(installedConfig, defaultConfig)) { ignerr << "Failed to copy installed config [" << installedConfig @@ -301,7 +305,7 @@ std::unique_ptr createGui( } } - // Also set ~/.ignition/gazebo/gui.config as the default path + // Also set ~/.ignition/gazebo/ver/gui.config as the default path if (!app->LoadConfig(defaultConfig)) { ignerr << "Failed to load config file[" << defaultConfig << "]." @@ -314,9 +318,11 @@ std::unique_ptr createGui( } ////////////////////////////////////////////////// -int runGui(int &_argc, char **_argv, const char *_guiConfig) +int runGui(int &_argc, char **_argv, const char *_guiConfig, + const char *_renderEngine) { - auto app = gazebo::gui::createGui(_argc, _argv, _guiConfig); + auto app = gazebo::gui::createGui( + _argc, _argv, _guiConfig, nullptr, true, _renderEngine); if (nullptr != app) { // Run main window. @@ -325,8 +331,8 @@ int runGui(int &_argc, char **_argv, const char *_guiConfig) igndbg << "Shutting down ign-gazebo-gui" << std::endl; return 0; } - else - return -1; + + return -1; } } // namespace gui } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc new file mode 100644 index 00000000000..dd89de306f1 --- /dev/null +++ b/src/gui/GuiEvents.cc @@ -0,0 +1,85 @@ +/* + * 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 "ignition/gazebo/gui/GuiEvents.hh" + +class ignition::gazebo::gui::events::GuiNewRemovedEntities::Implementation +{ + /// \brief Set of newly created entities + public: std::set newEntities; + + /// \brief Set of recently removed entities + public: std::set removedEntities; +}; + +class ignition::gazebo::gui::events::NewRemovedEntities::Implementation +{ + /// \brief Set of newly created entities + public: std::set newEntities; + + /// \brief Set of recently removed entities + public: std::set removedEntities; +}; + +using namespace ignition; +using namespace gazebo; +using namespace gui; +using namespace events; + +///////////////////////////////////////////////// +GuiNewRemovedEntities::GuiNewRemovedEntities( + const std::set &_newEntities, + const std::set &_removedEntities) + : QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->newEntities = _newEntities; + this->dataPtr->removedEntities = _removedEntities; +} + +///////////////////////////////////////////////// +const std::set &GuiNewRemovedEntities::NewEntities() const +{ + return this->dataPtr->newEntities; +} + +///////////////////////////////////////////////// +const std::set &GuiNewRemovedEntities::RemovedEntities() const +{ + return this->dataPtr->removedEntities; +} + +///////////////////////////////////////////////// +NewRemovedEntities::NewRemovedEntities( + const std::set &_newEntities, + const std::set &_removedEntities) + : QEvent(kType), dataPtr(utils::MakeImpl()) +{ + this->dataPtr->newEntities = _newEntities; + this->dataPtr->removedEntities = _removedEntities; +} + +///////////////////////////////////////////////// +const std::set &NewRemovedEntities::NewEntities() const +{ + return this->dataPtr->newEntities; +} + +///////////////////////////////////////////////// +const std::set &NewRemovedEntities::RemovedEntities() const +{ + return this->dataPtr->removedEntities; +} diff --git a/src/gui/GuiEvents_TEST.cc b/src/gui/GuiEvents_TEST.cc new file mode 100644 index 00000000000..cb0bebc9996 --- /dev/null +++ b/src/gui/GuiEvents_TEST.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 + +#include "ignition/gazebo/test_config.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +using namespace ignition; +using namespace gazebo; +using namespace gui; + +///////////////////////////////////////////////// +TEST(GuiEventsTest, GuiNewRemovedEntities) +{ + events::GuiNewRemovedEntities event({1, 2, 3}, {4, 5}); + + EXPECT_LT(QEvent::User, event.type()); + + auto addedEntities = event.NewEntities(); + EXPECT_EQ(3u, addedEntities.size()); + EXPECT_NE(addedEntities.find(1), addedEntities.end()); + EXPECT_NE(addedEntities.find(2), addedEntities.end()); + EXPECT_NE(addedEntities.find(3), addedEntities.end()); + EXPECT_EQ(addedEntities.find(100), addedEntities.end()); + + auto removedEntities = event.RemovedEntities(); + EXPECT_EQ(2u, removedEntities.size()); + EXPECT_NE(removedEntities.find(4), removedEntities.end()); + EXPECT_NE(removedEntities.find(5), removedEntities.end()); + EXPECT_EQ(removedEntities.find(6), removedEntities.end()); +} + +///////////////////////////////////////////////// +TEST(GuiEventsTest, NewRemovedEntities) +{ + events::NewRemovedEntities event({1, 2, 3}, {4, 5}); + + EXPECT_LT(QEvent::User, event.type()); + + auto addedEntities = event.NewEntities(); + EXPECT_EQ(3u, addedEntities.size()); + EXPECT_NE(addedEntities.find(1), addedEntities.end()); + EXPECT_NE(addedEntities.find(2), addedEntities.end()); + EXPECT_NE(addedEntities.find(3), addedEntities.end()); + EXPECT_EQ(addedEntities.find(100), addedEntities.end()); + + auto removedEntities = event.RemovedEntities(); + EXPECT_EQ(2u, removedEntities.size()); + EXPECT_NE(removedEntities.find(4), removedEntities.end()); + EXPECT_NE(removedEntities.find(5), removedEntities.end()); + EXPECT_EQ(removedEntities.find(6), removedEntities.end()); +} diff --git a/src/gui/GuiRunner.cc b/src/gui/GuiRunner.cc index e9788b33db0..50a2c3f0753 100644 --- a/src/gui/GuiRunner.cc +++ b/src/gui/GuiRunner.cc @@ -19,25 +19,29 @@ #include #include #include +#include #include +#include #include // Include all components so they have first-class support #include "ignition/gazebo/components/components.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/gui/GuiSystem.hh" +#include "GuiRunner.hh" + using namespace ignition; using namespace gazebo; +// Register SerializedStepMap to the Qt meta type system so we can pass objects +// of this type in QMetaObject::invokeMethod +Q_DECLARE_METATYPE(msgs::SerializedStepMap) + ///////////////////////////////////////////////// class ignition::gazebo::GuiRunner::Implementation { - /// \brief Update the plugins. - public: void UpdatePlugins(); - /// \brief Entity-component manager. public: gazebo::EntityComponentManager ecm; @@ -58,14 +62,36 @@ class ignition::gazebo::GuiRunner::Implementation /// \brief The plugin update thread.. public: std::thread updateThread; + + /// \brief True if the initial state has been received and processed. + public: bool receivedInitialState{false}; + + /// \brief Name of WorldControl service + public: std::string controlService; }; ///////////////////////////////////////////////// GuiRunner::GuiRunner(const std::string &_worldName) : dataPtr(utils::MakeUniqueImpl()) { + qRegisterMetaType(); + this->setProperty("worldName", QString::fromStdString(_worldName)); + // Allow for creation of entities on GUI side. + // Note we have to start the entity id at an offset so it does not conflict + // with the ones on the server. The log playback starts at max 64bit int / 2 + // On the gui side, we will start entity id at an offset of max 32bit int / 4 + // because currently many plugins cast entity ids to a 32 bit signed/unsigned + // int. + // todo(anyone) fix all gui plugins to use 64bit unsigned int for Entity ids + // and add support for accepting uint64_t data in ign-rendering Node's + // UserData object. + // todo(anyone) address + // https://github.com/ignitionrobotics/ign-gazebo/issues/1134 + // so that an offset is not required + this->dataPtr->ecm.SetEntityCreateOffset(math::MAX_I32 / 2); + auto win = gui::App()->findChild(); auto winWorldNames = win->property("worldNames").toStringList(); winWorldNames.append(QString::fromStdString(_worldName)); @@ -91,28 +117,54 @@ GuiRunner::GuiRunner(const std::string &_worldName) this->RequestState(); // Periodically update the plugins - // \todo(anyone) Move the global variables to GuiRunner::Implementation on v5 - this->dataPtr->running = true; - this->dataPtr->updateThread = std::thread([&]() - { - while (this->dataPtr->running) - { - { - std::lock_guard lock(this->dataPtr->updateMutex); - this->dataPtr->UpdatePlugins(); - } - // This is roughly a 30Hz update rate. - std::this_thread::sleep_for(std::chrono::milliseconds(33)); - } - }); + QPointer timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, &GuiRunner::UpdatePlugins); + timer->start(33); + + this->dataPtr->controlService = "/world/" + _worldName + "/control/state"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); } ///////////////////////////////////////////////// -GuiRunner::~GuiRunner() +GuiRunner::~GuiRunner() = default; + +///////////////////////////////////////////////// +bool GuiRunner::eventFilter(QObject *_obj, QEvent *_event) { - this->dataPtr->running = false; - if (this->dataPtr->updateThread.joinable()) - this->dataPtr->updateThread.join(); + if (_event->type() == ignition::gui::events::WorldControl::kType) + { + auto worldControlEvent = + reinterpret_cast(_event); + if (worldControlEvent) + { + msgs::WorldControlState req; + req.mutable_world_control()->CopyFrom( + worldControlEvent->WorldControlInfo()); + + // share the GUI's ECM with the server if: + // 1. Play was pressed + // 2. Step was pressed while paused + const auto &info = worldControlEvent->WorldControlInfo(); + const bool pressedStep = info.multi_step() > 0u; + const bool pressedPlay = !info.pause() && !pressedStep; + const bool pressedStepWhilePaused = info.pause() && pressedStep; + if (pressedPlay || pressedStepWhilePaused) + req.mutable_state()->CopyFrom(this->dataPtr->ecm.State()); + + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sharing WorldControl info with the server.\n"; + }; + this->dataPtr->node.Request(this->dataPtr->controlService, req, cb); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); } ///////////////////////////////////////////////// @@ -145,6 +197,10 @@ void GuiRunner::RequestState() ignition::msgs::StringMsg req; req.set_data(reqSrv); + // Subscribe to periodic updates. + this->dataPtr->node.Subscribe(this->dataPtr->stateTopic, + &GuiRunner::OnState, this); + // send async state request this->dataPtr->node.Request(this->dataPtr->stateTopic + "_async", req); } @@ -159,7 +215,13 @@ void GuiRunner::OnPluginAdded(const QString &) ///////////////////////////////////////////////// void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) { - this->OnState(_res); + // Since this function may be called from a transport thread, we push the + // OnStateQt function to the queue so that its called from the Qt thread. This + // ensures that only one thread has access to the ecm and updateInfo + // variables. + QMetaObject::invokeMethod(this, "OnStateQt", Qt::QueuedConnection, + Q_ARG(msgs::SerializedStepMap, _res)); + this->dataPtr->receivedInitialState = true; // todo(anyone) store reqSrv string in a member variable and use it here // and in RequestState() @@ -167,13 +229,6 @@ void GuiRunner::OnStateAsyncService(const msgs::SerializedStepMap &_res) std::string reqSrv = this->dataPtr->node.Options().NameSpace() + "/" + id + "/state_async"; this->dataPtr->node.UnadvertiseSrv(reqSrv); - - // Only subscribe to periodic updates after receiving initial state - if (this->dataPtr->node.SubscribedTopics().empty()) - { - this->dataPtr->node.Subscribe(this->dataPtr->stateTopic, - &GuiRunner::OnState, this); - } } ///////////////////////////////////////////////// @@ -182,23 +237,39 @@ void GuiRunner::OnState(const msgs::SerializedStepMap &_msg) IGN_PROFILE_THREAD_NAME("GuiRunner::OnState"); IGN_PROFILE("GuiRunner::Update"); - std::lock_guard lock(this->dataPtr->updateMutex); + // Only process state updates after initial state has been received. + if (!this->dataPtr->receivedInitialState) + return; + + // Since this function may be called from a transport thread, we push the + // OnStateQt function to the queue so that its called from the Qt thread. This + // ensures that only one thread has access to the ecm and updateInfo + // variables. + QMetaObject::invokeMethod(this, "OnStateQt", Qt::QueuedConnection, + Q_ARG(msgs::SerializedStepMap, _msg)); +} + +///////////////////////////////////////////////// +void GuiRunner::OnStateQt(const msgs::SerializedStepMap &_msg) +{ + IGN_PROFILE_THREAD_NAME("Qt thread"); + IGN_PROFILE("GuiRunner::Update"); this->dataPtr->ecm.SetState(_msg.state()); // Update all plugins this->dataPtr->updateInfo = convert(_msg.stats()); - this->dataPtr->UpdatePlugins(); - this->dataPtr->ecm.ClearNewlyCreatedEntities(); - this->dataPtr->ecm.ProcessRemoveEntityRequests(); + this->UpdatePlugins(); } ///////////////////////////////////////////////// -void GuiRunner::Implementation::UpdatePlugins() +void GuiRunner::UpdatePlugins() { auto plugins = gui::App()->findChildren(); for (auto plugin : plugins) { - plugin->Update(this->updateInfo, this->ecm); + plugin->Update(this->dataPtr->updateInfo, this->dataPtr->ecm); } - this->ecm.ClearRemovedComponents(); + this->dataPtr->ecm.ClearRemovedComponents(); + this->dataPtr->ecm.ClearNewlyCreatedEntities(); + this->dataPtr->ecm.ProcessRemoveEntityRequests(); } diff --git a/include/ignition/gazebo/gui/GuiRunner.hh b/src/gui/GuiRunner.hh similarity index 78% rename from include/ignition/gazebo/gui/GuiRunner.hh rename to src/gui/GuiRunner.hh index 381e9deda66..1a060981615 100644 --- a/include/ignition/gazebo/gui/GuiRunner.hh +++ b/src/gui/GuiRunner.hh @@ -41,13 +41,14 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \brief Constructor /// \param[in] _worldName World name. - /// \todo Move to src/gui on v6. - public: explicit IGN_DEPRECATED(5.0) GuiRunner( - const std::string &_worldName); + public: explicit GuiRunner(const std::string &_worldName); /// \brief Destructor public: ~GuiRunner() override; + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + /// \brief Callback when a plugin has been added. /// This function has no effect and is left here for ABI compatibility. /// \param[in] _objectName Plugin's object name. @@ -60,10 +61,19 @@ class IGNITION_GAZEBO_GUI_VISIBLE GuiRunner : public QObject /// \param[in] _res Response containing new state. private: void OnStateAsyncService(const msgs::SerializedStepMap &_res); - /// \brief Callback when a new state is received from the server. + /// \brief Callback when a new state is received from the server. Actual + /// updating of the ECM is delegated to OnStateQt /// \param[in] _msg New state message. private: void OnState(const msgs::SerializedStepMap &_msg); + /// \brief Called by the Qt thread to update the ECM with new state + /// \param[in] _msg New state message. + private: Q_INVOKABLE void OnStateQt(const msgs::SerializedStepMap &_msg); + + /// \brief Update the plugins. + /// \todo(anyone) Move to GuiRunner::Implementation when porting to v5 + private: Q_INVOKABLE void UpdatePlugins(); + /// \brief Pointer to private data. IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) }; diff --git a/src/gui/Gui_TEST.cc b/src/gui/Gui_TEST.cc index 96bd3c952bd..881bfb931b5 100644 --- a/src/gui/Gui_TEST.cc +++ b/src/gui/Gui_TEST.cc @@ -90,7 +90,8 @@ TEST_F(GuiTest, IGN_UTILS_TEST_DISABLED_ON_MAC(PathManager)) node.Advertise("/gazebo/resource_paths/get", pathsCb); igndbg << "Paths advertised" << std::endl; - auto app = ignition::gazebo::gui::createGui(gg_argc, gg_argv, nullptr); + auto app = ignition::gazebo::gui::createGui( + gg_argc, gg_argv, nullptr, nullptr, false, nullptr); EXPECT_NE(nullptr, app); igndbg << "GUI created" << std::endl; diff --git a/src/gui/TmpIface.cc b/src/gui/TmpIface.cc deleted file mode 100644 index e945569510a..00000000000 --- a/src/gui/TmpIface.cc +++ /dev/null @@ -1,102 +0,0 @@ -/* - * 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/gui/TmpIface.hh" - -using namespace ignition; -using namespace gazebo; - -///////////////////////////////////////////////// -TmpIface::TmpIface() -{ - // Server control - this->node.Advertise("/server_control", - &TmpIface::OnServerControl, this); -} - -///////////////////////////////////////////////// -bool TmpIface::OnServerControl(const msgs::ServerControl &_req, - msgs::Boolean &_res) -{ - igndbg << "OnServerControl: request" << std::endl; - igndbg << _req.DebugString() << std::endl; - - _res.set_data(true); - - igndbg << "OnServerControl: response" << std::endl; - igndbg << _res.DebugString() << std::endl; - - return true; -} - -///////////////////////////////////////////////// -void TmpIface::OnNewWorld() -{ - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) - { - igndbg << "NewWorld callback: result: " << _result << std::endl; - igndbg << "NewWorld callback: response: " << _res.DebugString() - << std::endl; - }; - - msgs::ServerControl req; - req.set_new_world(true); - this->node.Request("/server_control", req, cb); -} - -///////////////////////////////////////////////// -void TmpIface::OnLoadWorld(const QString &_path) -{ - auto localPath = QUrl(_path).toLocalFile(); - if (localPath.isEmpty()) - localPath = _path; - - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) - { - igndbg << "LoadWorld callback: result: " << _result << std::endl; - igndbg << "LoadWorld callback: response: " << _res.DebugString() - << std::endl; - }; - - msgs::ServerControl req; - req.set_open_filename(localPath.toStdString()); - this->node.Request("/server_control", req, cb); -} - -///////////////////////////////////////////////// -void TmpIface::OnSaveWorldAs(const QString &_path) -{ - auto localPath = QUrl(_path).toLocalFile(); - if (localPath.isEmpty()) - localPath = _path; - - std::function cb = - [](const ignition::msgs::Boolean &_res, const bool _result) - { - igndbg << "SaveWorldAs callback: result: " << _result << std::endl; - igndbg << "SaveWorldAs callback: response: " << _res.DebugString() - << std::endl; - }; - - msgs::ServerControl req; - req.set_save_filename(localPath.toStdString()); - this->node.Request("/server_control", req, cb); -} diff --git a/src/gui/gui.config b/src/gui/gui.config index 137c55e6483..b6296bd2070 100644 --- a/src/gui/gui.config +++ b/src/gui/gui.config @@ -27,7 +27,7 @@ - + 3D View false @@ -38,10 +38,85 @@ scene 0.4 0.4 0.4 0.8 0.8 0.8 - 6 0 6 0 0.5 3.14 + -6 0 6 0 0.5 0 - + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + + + false + 5 + 5 + floating + false + + + + World control @@ -61,10 +136,11 @@ true true true + true - + World stats @@ -85,7 +161,6 @@ true true true - @@ -128,6 +203,9 @@ false #777777 + + + false @@ -144,6 +222,20 @@ + + + + false + 300 + 50 + 100 + 50 + floating + false + #777777 + + + diff --git a/src/gui/playback_gui.config b/src/gui/playback_gui.config index 50cffa6992d..4e6d3864475 100644 --- a/src/gui/playback_gui.config +++ b/src/gui/playback_gui.config @@ -60,6 +60,7 @@ true true true + true diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 1c854c24259..bcceed233c1 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -121,8 +121,9 @@ add_subdirectory(modules) add_subdirectory(align_tool) add_subdirectory(banana_for_scale) add_subdirectory(component_inspector) +add_subdirectory(copy_paste) +add_subdirectory(entity_context_menu) add_subdirectory(entity_tree) -add_subdirectory(grid_config) add_subdirectory(joint_position_controller) add_subdirectory(lights) add_subdirectory(playback_scrubber) @@ -130,10 +131,13 @@ add_subdirectory(plot_3d) add_subdirectory(plotting) add_subdirectory(resource_spawner) add_subdirectory(scene3d) +add_subdirectory(select_entities) +add_subdirectory(scene_manager) add_subdirectory(shapes) -add_subdirectory(tape_measure) +add_subdirectory(spawn) add_subdirectory(transform_control) add_subdirectory(video_recorder) add_subdirectory(view_angle) +add_subdirectory(visualization_capabilities) add_subdirectory(visualize_contacts) add_subdirectory(visualize_lidar) diff --git a/src/gui/plugins/align_tool/AlignTool.cc b/src/gui/plugins/align_tool/AlignTool.cc index c605edd27b9..a2d965ef26c 100644 --- a/src/gui/plugins/align_tool/AlignTool.cc +++ b/src/gui/plugins/align_tool/AlignTool.cc @@ -364,7 +364,8 @@ void AlignTool::Align() if (!vis) continue; - if (std::get(vis->UserData("gazebo-entity")) == + if (vis->HasUserData("gazebo-entity") && + std::get(vis->UserData("gazebo-entity")) == static_cast(entityId)) { // Check here to see if visual is top level or not, continue if not diff --git a/src/gui/plugins/banana_for_scale/BananaForScale.cc b/src/gui/plugins/banana_for_scale/BananaForScale.cc index af2e262f8f2..469a442950c 100644 --- a/src/gui/plugins/banana_for_scale/BananaForScale.cc +++ b/src/gui/plugins/banana_for_scale/BananaForScale.cc @@ -26,7 +26,6 @@ #include #include #include -#include #include #include @@ -106,13 +105,6 @@ void BananaForScale::OnMode(const QString &_mode) ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &event); - - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - ignition::gazebo::gui::events::SpawnPreviewPath oldEvent(sdfPath); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &oldEvent); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // Register this plugin diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 32554165655..5dbfff76b4a 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -16,7 +16,9 @@ */ #include +#include #include +#include #include #include #include @@ -27,12 +29,15 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/AngularAcceleration.hh" #include "ignition/gazebo/components/AngularVelocity.hh" +#include "ignition/gazebo/components/BatterySoC.hh" #include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/CenterOfVolume.hh" #include "ignition/gazebo/components/ChildLinkName.hh" #include "ignition/gazebo/components/Collision.hh" #include "ignition/gazebo/components/Factory.hh" #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/LaserRetro.hh" #include "ignition/gazebo/components/Level.hh" #include "ignition/gazebo/components/Light.hh" #include "ignition/gazebo/components/LightCmd.hh" @@ -42,6 +47,7 @@ #include "ignition/gazebo/components/LinearVelocitySeed.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/MagneticField.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" @@ -57,8 +63,12 @@ #include "ignition/gazebo/components/SelfCollide.hh" #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/ThreadPitch.hh" +#include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/components/Volume.hh" #include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -240,6 +250,13 @@ void ignition::gazebo::setData(QStandardItem *_item, const int &_data) _item->setData(_data, ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, const Entity &_data) +{ + setData(_item, static_cast(_data)); +} + ////////////////////////////////////////////////// template<> void ignition::gazebo::setData(QStandardItem *_item, const double &_data) @@ -267,6 +284,59 @@ void ignition::gazebo::setData(QStandardItem *_item, const sdf::Physics &_data) }), ComponentsModel::RoleNames().key("data")); } +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, + const sdf::Material &_data) +{ + if (nullptr == _item) + return; + + _item->setData(QString("Material"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(_data.Ambient().R() * 255), + QVariant(_data.Ambient().G() * 255), + QVariant(_data.Ambient().B() * 255), + QVariant(_data.Ambient().A() * 255), + QVariant(_data.Diffuse().R() * 255), + QVariant(_data.Diffuse().G() * 255), + QVariant(_data.Diffuse().B() * 255), + QVariant(_data.Diffuse().A() * 255), + QVariant(_data.Specular().R() * 255), + QVariant(_data.Specular().G() * 255), + QVariant(_data.Specular().B() * 255), + QVariant(_data.Specular().A() * 255), + QVariant(_data.Emissive().R() * 255), + QVariant(_data.Emissive().G() * 255), + QVariant(_data.Emissive().B() * 255), + QVariant(_data.Emissive().A() * 255) + }), ComponentsModel::RoleNames().key("data")); + + // TODO(anyone) Only shows colors of material, + // need to add others (e.g., pbr) +} + +////////////////////////////////////////////////// +template<> +void ignition::gazebo::setData(QStandardItem *_item, + const math::SphericalCoordinates &_data) +{ + if (nullptr == _item) + return; + + _item->setData(QString("SphericalCoordinates"), + ComponentsModel::RoleNames().key("dataType")); + _item->setData(QList({ + QVariant(QString::fromStdString(math::SphericalCoordinates::Convert( + _data.Surface()))), + QVariant(_data.LatitudeReference().Degree()), + QVariant(_data.LongitudeReference().Degree()), + QVariant(_data.ElevationReference()), + QVariant(_data.HeadingOffset().Degree()), + }), ComponentsModel::RoleNames().key("data")); +} + ////////////////////////////////////////////////// void ignition::gazebo::setUnit(QStandardItem *_item, const std::string &_unit) { @@ -366,6 +436,7 @@ ComponentInspector::ComponentInspector() : GuiSystem(), dataPtr(std::make_unique()) { qRegisterMetaType(); + qRegisterMetaType("Entity"); } ///////////////////////////////////////////////// @@ -482,12 +553,7 @@ void ComponentInspector::Update(const UpdateInfo &, // Add component to list else { - // TODO(louise) Blocking here is not the best idea - QMetaObject::invokeMethod(&this->dataPtr->componentsModel, - "AddComponentType", - Qt::BlockingQueuedConnection, - Q_RETURN_ARG(QStandardItem *, item), - Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); + item = this->dataPtr->componentsModel.AddComponentType(typeId); } item->setData(QString::number(this->dataPtr->entity), @@ -528,6 +594,13 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::BatterySoC::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::CastShadows::typeId) { auto comp = _ecm.Component( @@ -535,6 +608,16 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::CenterOfVolume::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + { + setData(item, comp->Data()); + setUnit(item, "m"); + } + } else if (typeId == components::ChildLinkName::typeId) { auto comp = _ecm.Component( @@ -551,6 +634,12 @@ void ComponentInspector::Update(const UpdateInfo &, setUnit(item, "m/s\u00B2"); } } + else if (typeId == components::LaserRetro::typeId) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } else if (typeId == components::LinearAcceleration::typeId) { auto comp = _ecm.Component( @@ -695,6 +784,40 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::SphericalCoordinates::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::ThreadPitch::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + { + setData(item, comp->Data()); + setUnit(item, "m"); + } + } + else if (typeId == components::Transparency::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + setData(item, comp->Data()); + } + else if (typeId == components::Volume::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + { + setData(item, comp->Data()); + setUnit(item, "m\u00B3"); + } + } else if (typeId == components::WindMode::typeId) { auto comp = _ecm.Component(this->dataPtr->entity); @@ -711,6 +834,16 @@ void ComponentInspector::Update(const UpdateInfo &, setUnit(item, "rad/s\u00B2"); } } + else if (typeId == components::WorldAngularVelocity::typeId) + { + auto comp = _ecm.Component( + this->dataPtr->entity); + if (comp) + { + setData(item, comp->Data()); + setUnit(item, "rad/s"); + } + } else if (typeId == components::WorldLinearVelocity::typeId) { auto comp = _ecm.Component( @@ -744,20 +877,36 @@ void ComponentInspector::Update(const UpdateInfo &, if (comp) setData(item, comp->Data()); } + else if (typeId == components::Material::typeId) + { + auto comp = _ecm.Component(this->dataPtr->entity); + if (comp) + { + this->SetType("material"); + setData(item, comp->Data()); + } + } } - // Remove components no longer present + // Remove components no longer present - list items to remove + std::list itemsToRemove; for (auto itemIt : this->dataPtr->componentsModel.items) { auto typeId = itemIt.first; if (componentTypes.find(typeId) == componentTypes.end()) { - QMetaObject::invokeMethod(&this->dataPtr->componentsModel, - "RemoveComponentType", - Qt::QueuedConnection, - Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); + itemsToRemove.push_back(typeId); } } + + // Remove components in list + for (auto typeId : itemsToRemove) + { + QMetaObject::invokeMethod(&this->dataPtr->componentsModel, + "RemoveComponentType", + Qt::QueuedConnection, + Q_ARG(ignition::gazebo::ComponentTypeId, typeId)); + } } ///////////////////////////////////////////////// @@ -790,13 +939,13 @@ bool ComponentInspector::eventFilter(QObject *_obj, QEvent *_event) } ///////////////////////////////////////////////// -int ComponentInspector::Entity() const +Entity ComponentInspector::GetEntity() const { return this->dataPtr->entity; } ///////////////////////////////////////////////// -void ComponentInspector::SetEntity(const int &_entity) +void ComponentInspector::SetEntity(const Entity &_entity) { // If nothing is selected, display world properties if (_entity == kNullEntity) @@ -954,6 +1103,134 @@ void ComponentInspector::OnPhysics(double _stepSize, double _realTimeFactor) this->dataPtr->node.Request(physicsCmdService, req, cb); } +///////////////////////////////////////////////// +void ComponentInspector::OnMaterialColor( + double _rAmbient, double _gAmbient, double _bAmbient, double _aAmbient, + double _rDiffuse, double _gDiffuse, double _bDiffuse, double _aDiffuse, + double _rSpecular, double _gSpecular, double _bSpecular, double _aSpecular, + double _rEmissive, double _gEmissive, double _bEmissive, double _aEmissive, + QString _type, QColor _currColor) +{ + // when type is not empty, open qt color dialog + std::string type = _type.toStdString(); + if (!type.empty()) + { + QColor newColor = QColorDialog::getColor( + _currColor, nullptr, "Pick a color", + {QColorDialog::DontUseNativeDialog, QColorDialog::ShowAlphaChannel}); + + // returns if the user hits cancel + if (!newColor.isValid()) + return; + + if (type == "ambient") + { + _rAmbient = newColor.red(); + _gAmbient = newColor.green(); + _bAmbient = newColor.blue(); + _aAmbient = newColor.alpha(); + } + else if (type == "diffuse") + { + _rDiffuse = newColor.red(); + _gDiffuse = newColor.green(); + _bDiffuse = newColor.blue(); + _aDiffuse = newColor.alpha(); + } + else if (type == "specular") + { + _rSpecular = newColor.red(); + _gSpecular = newColor.green(); + _bSpecular = newColor.blue(); + _aSpecular = newColor.alpha(); + } + else if (type == "emissive") + { + _rEmissive = newColor.red(); + _gEmissive = newColor.green(); + _bEmissive = newColor.blue(); + _aEmissive = newColor.alpha(); + } + else + { + ignerr << "Invalid material type: " << type << std::endl; + return; + } + } + + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting material color configuration" + << " on visual" << std::endl; + }; + + msgs::Visual req; + req.set_id(this->dataPtr->entity); + + msgs::Set(req.mutable_material()->mutable_ambient(), + math::Color(_rAmbient / 255.0, _gAmbient / 255.0, + _bAmbient / 255.0, _aAmbient / 255.0)); + msgs::Set(req.mutable_material()->mutable_diffuse(), + math::Color(_rDiffuse / 255.0, _gDiffuse / 255.0, + _bDiffuse / 255.0, _aDiffuse / 255.0)); + msgs::Set(req.mutable_material()->mutable_specular(), + math::Color(_rSpecular / 255.0, _gSpecular / 255.0, + _bSpecular / 255.0, _aSpecular / 255.0)); + msgs::Set(req.mutable_material()->mutable_emissive(), + math::Color(_rEmissive / 255.0, _gEmissive / 255.0, + _bEmissive / 255.0, _aEmissive / 255.0)); + + auto materialCmdService = "/world/" + this->dataPtr->worldName + + "/visual_config"; + materialCmdService = transport::TopicUtils::AsValidTopic(materialCmdService); + if (materialCmdService.empty()) + { + ignerr << "Invalid material command service topic provided" << std::endl; + return; + } + this->dataPtr->node.Request(materialCmdService, req, cb); +} + +///////////////////////////////////////////////// +void ComponentInspector::OnSphericalCoordinates(QString _surface, + double _latitude, double _longitude, double _elevation, + double _heading) +{ + if (_surface != QString("EARTH_WGS84")) + { + ignerr << "Surface [" << _surface.toStdString() << "] not supported." + << std::endl; + return; + } + + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting spherical coordinates." << std::endl; + }; + + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(_latitude); + req.set_longitude_deg(_longitude); + req.set_elevation(_elevation); + req.set_heading_deg(_heading); + + auto sphericalCoordsCmdService = "/world/" + this->dataPtr->worldName + + "/set_spherical_coordinates"; + sphericalCoordsCmdService = + transport::TopicUtils::AsValidTopic(sphericalCoordsCmdService); + if (sphericalCoordsCmdService.empty()) + { + ignerr << "Invalid spherical coordinates service" << std::endl; + return; + } + this->dataPtr->node.Request(sphericalCoordsCmdService, 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 193f7b4ddd2..2b8bbd50c7d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -21,6 +21,10 @@ #include #include #include + +#include +#include + #include #include @@ -28,8 +32,6 @@ #include #include -#include "ignition/gazebo/components/Physics.hh" - #include Q_DECLARE_METATYPE(ignition::gazebo::ComponentTypeId) @@ -91,6 +93,12 @@ namespace gazebo template<> void setData(QStandardItem *_item, const sdf::Physics &_data); + /// \brief Specialized to set Spherical Coordinates data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const math::SphericalCoordinates &_data); + /// \brief Specialized to set boolean data. /// \param[in] _item Item whose data will be set. /// \param[in] _data Data to set. @@ -115,6 +123,13 @@ namespace gazebo template<> void setData(QStandardItem *_item, const std::ostream &_data); + /// \brief Specialized to set material data. + /// \param[in] _item Item whose data will be set. + /// \param[in] _data Data to set. + template<> + void setData(QStandardItem *_item, const sdf::Material &_data); + + /// \brief Set the unit of a given item. /// \param[in] _item Item whose unit will be set. /// \param[in] _unit Unit to be displayed, such as 'm' for meters. @@ -164,8 +179,8 @@ namespace gazebo /// \brief Entity Q_PROPERTY( - int entity - READ Entity + Entity entity + READ GetEntity WRITE SetEntity NOTIFY EntityChanged ) @@ -260,6 +275,45 @@ namespace gazebo public: Q_INVOKABLE void OnPhysics(double _stepSize, double _realTimeFactor); + // \brief Callback in Qt thread when material color changes for a visual + /// \param[in] _rAmbient ambient red + /// \param[in] _gAmbient ambient green + /// \param[in] _bAmbient ambient blue + /// \param[in] _aAmbient ambient alpha + /// \param[in] _rDiffuse diffuse red + /// \param[in] _gDiffuse diffuse green + /// \param[in] _bDiffuse diffuse blue + /// \param[in] _aDiffuse diffuse alpha + /// \param[in] _rSpecular specular red + /// \param[in] _gSpecular specular green + /// \param[in] _bSpecular specular blue + /// \param[in] _aSpecular specular alpha + /// \param[in] _rEmissive emissive red + /// \param[in] _gEmissive emissive green + /// \param[in] _bEmissive emissive blue + /// \param[in] _aEmissive emissive alpha + /// \param[in] _type if type is not empty, opens QColorDialog. + /// The possible types are ambient, diffuse, specular, or emissive. + /// \param[in] _currColor used for QColorDialog to show the current color + /// in the open dialog. + public: Q_INVOKABLE void OnMaterialColor( + double _rAmbient, double _gAmbient, double _bAmbient, + double _aAmbient, double _rDiffuse, double _gDiffuse, + double _bDiffuse, double _aDiffuse, double _rSpecular, + double _gSpecular, double _bSpecular, double _aSpecular, + double _rEmissive, double _gEmissive, double _bEmissive, + double _aEmissive, QString _type, QColor _currColor); + + /// \brief Callback in Qt thread when spherical coordinates change. + /// \param[in] _surface Surface model + /// \param[in] _latitude Latitude in degrees + /// \param[in] _longitude Longitude in degrees + /// \param[in] _elevation Elevation in meters + /// \param[in] _heading Heading in degrees + public: Q_INVOKABLE void OnSphericalCoordinates(QString _surface, + double _latitude, double _longitude, double _elevation, + double _heading); + /// \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; @@ -272,11 +326,11 @@ namespace gazebo /// \brief Get the entity currently inspected. /// \return Entity ID. - public: Q_INVOKABLE int Entity() const; + public: Q_INVOKABLE Entity GetEntity() const; /// \brief Set the entity currently inspected. /// \param[in] _entity Entity ID. - public: Q_INVOKABLE void SetEntity(const int &_entity); + public: Q_INVOKABLE void SetEntity(const Entity &_entity); /// \brief Notify that entity has changed. signals: void EntityChanged(); diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qml b/src/gui/plugins/component_inspector/ComponentInspector.qml index bb59610008c..53a77abfd3d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -78,6 +78,15 @@ Rectangle { return _model.dataType + '.qml' } + // Get number of decimal digits based on a widget's width + function getDecimals(_width) { + if (_width <= 80) + return 2; + else if (_width <= 100) + return 4; + return 6; + } + /** * Forward pose changes to C++ */ @@ -107,6 +116,30 @@ Rectangle { ComponentInspector.OnPhysics(_stepSize, _realTimeFactor) } + /** + * Forward material color changes to C++ + */ + function onMaterialColor(_rAmbient, _gAmbient, _bAmbient, _aAmbient, + _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, + _rSpecular, _gSpecular, _bSpecular, _aSpecular, + _rEmissive, _gEmissive, _bEmissive, _aEmissive, + _type, _currColor) { + ComponentInspector.OnMaterialColor( + _rAmbient, _gAmbient, _bAmbient, _aAmbient, + _rDiffuse, _gDiffuse, _bDiffuse, _aDiffuse, + _rSpecular, _gSpecular, _bSpecular, _aSpecular, + _rEmissive, _gEmissive, _bEmissive, _aEmissive, + _type, _currColor) + } + + /* + * Forward spherical coordinate changes to C++ + */ + function onSphericalCoordinates(_surface, _lat, _lon, _elevation, _heading) { + ComponentInspector.OnSphericalCoordinates(_surface, _lat, _lon, _elevation, + _heading); + } + 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 1996720cbcd..79b7d8a30d2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -2,10 +2,15 @@ Boolean.qml ComponentInspector.qml + ExpandingTypeHeader.qml + Float.qml + Integer.qml Light.qml NoData.qml + Material.qml Physics.qml Pose3d.qml + SphericalCoordinates.qml String.qml TypeHeader.qml Vector3d.qml diff --git a/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml new file mode 100644 index 00000000000..c233ecf7752 --- /dev/null +++ b/src/gui/plugins/component_inspector/ExpandingTypeHeader.qml @@ -0,0 +1,72 @@ +/* + * 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.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 + +// Header +Rectangle { + id: header + width: parent.width + height: typeHeader.height + color: "transparent" + + // Horizontal margins + property int margin: 5 + + // Left indentation + property int indentation: 10 + + 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" + } + } +} diff --git a/src/gui/plugins/component_inspector/Float.qml b/src/gui/plugins/component_inspector/Float.qml index a72d7801bef..f01cfb8e35c 100644 --- a/src/gui/plugins/component_inspector/Float.qml +++ b/src/gui/plugins/component_inspector/Float.qml @@ -38,6 +38,9 @@ Rectangle { // Maximum spinbox value property double spinMax: 1000000 + // Unit + property string unit: model && model.unit != undefined ? model.unit : '' + RowLayout { anchors.fill: parent @@ -55,13 +58,20 @@ Rectangle { id: typeHeader } - IgnSpinBox { + // TODO(anyone) Support write mode + Text { id: content - value: model.data - minimumValue: -spinMax - maximumValue: spinMax - decimals: xSpin.width < 100 ? 2 : 6 Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: Material.theme == Material.Light ? "black" : "white" + font.pointSize: 12 + text: { + var decimals = getDecimals(content.width) + var valueText = model.data.toFixed(decimals) + if (unit !== '') + valueText += ' ' + unit + return valueText + } } Item { diff --git a/src/gui/plugins/component_inspector/Integer.qml b/src/gui/plugins/component_inspector/Integer.qml index 316c2b2e7c6..12c9b5174e2 100644 --- a/src/gui/plugins/component_inspector/Integer.qml +++ b/src/gui/plugins/component_inspector/Integer.qml @@ -38,7 +38,10 @@ Rectangle { // Maximum spinbox value property double spinMax: 1000000 - Row { + // Unit + property string unit: model && model.unit != undefined ? model.unit : '' + + RowLayout { anchors.fill: parent Item { @@ -55,12 +58,19 @@ Rectangle { id: typeHeader } - IgnSpinBox { + // TODO(anyone) Support write mode + Text { id: content - value: model.data - minimumValue: -spinMax - maximumValue: spinMax Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: Material.theme == Material.Light ? "black" : "white" + font.pointSize: 12 + text: { + var valueText = model.data + if (unit !== '') + valueText += ' ' + unit + return valueText + } } Item { diff --git a/src/gui/plugins/component_inspector/Light.qml b/src/gui/plugins/component_inspector/Light.qml index 15881a434a9..d76dedcec58 100644 --- a/src/gui/plugins/component_inspector/Light.qml +++ b/src/gui/plugins/component_inspector/Light.qml @@ -221,48 +221,11 @@ Rectangle { Column { anchors.fill: parent - // Header - Rectangle { + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { 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" - } - } + // Using the default header text values. } // Content diff --git a/src/gui/plugins/component_inspector/Material.qml b/src/gui/plugins/component_inspector/Material.qml new file mode 100644 index 00000000000..a7d82cc6c87 --- /dev/null +++ b/src/gui/plugins/component_inspector/Material.qml @@ -0,0 +1,606 @@ +/* + * 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 material color 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 items for ambient red, green, blue, alpha + property var rAmbientItem: {} + property var gAmbientItem: {} + property var bAmbientItem: {} + property var aAmbientItem: {} + + // Loaded items for diffuse red, green, blue, alpha + property var rDiffuseItem: {} + property var gDiffuseItem: {} + property var bDiffuseItem: {} + property var aDiffuseItem: {} + + // Loaded items for specular red, green, blue, alpha + property var rSpecularItem: {} + property var gSpecularItem: {} + property var bSpecularItem: {} + property var aSpecularItem: {} + + // Loaded items for emissive red, green, blue, alpha + property var rEmissiveItem: {} + property var gEmissiveItem: {} + property var bEmissiveItem: {} + property var aEmissiveItem: {} + + // send new material color data to C++ + function sendMaterialColor(_type, _currColor) { + componentInspector.onMaterialColor( + rAmbientItem.value, + gAmbientItem.value, + bAmbientItem.value, + aAmbientItem.value, + rDiffuseItem.value, + gDiffuseItem.value, + bDiffuseItem.value, + aDiffuseItem.value, + rSpecularItem.value, + gSpecularItem.value, + bSpecularItem.value, + aSpecularItem.value, + rEmissiveItem.value, + gEmissiveItem.value, + bEmissiveItem.value, + aEmissiveItem.value, + _type, + _currColor + ); + } + + // Get button color from model.data, related start indices + // 0 = ambient + // 4 = diffuse + // 8 = specular + // 12 = emissive + function getButtonColor(_start_index, _isFillColor) + { + if (_isFillColor) { + // fill color (of object in the scene) + return Qt.rgba(model.data[_start_index], + model.data[_start_index + 1], + model.data[_start_index + 2], + model.data[_start_index + 3]) + } else { + // border color's alpha set to 1 incase fill color is 0 + return Qt.rgba(model.data[_start_index], + model.data[_start_index + 1], + model.data[_start_index + 2], + 1.0) + } + } + + FontMetrics { + id: fontMetrics + font.family: "Roboto" + } + + // Used to create rgba spin boxes + Component { + id: spinBoxMaterialColor + IgnSpinBox { + id: writableSpin + value: writableSpin.activeFocus ? writableSpin.value : numberValue + minimumValue: 0 + maximumValue: 255 + decimals: 0 + onEditingFinished: { + // sending empty params to not open color dialog + sendMaterialColor("", Qt.rgba(0, 0, 0, 0)) + } + } + } + + 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 + spacing: 20 + + GridLayout { + width: parent.width + columns: 6 + + // rgba headers + Text { + text: "Red " + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + Layout.row: 1 + Layout.column: 3 + } + Text { + text: "Green" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + Layout.row: 1 + Layout.column: 4 + } + Text { + text: "Blue " + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + Layout.row: 1 + Layout.column: 5 + } + Text { + text: "Alpha" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + Layout.row: 1 + Layout.column: 6 + } + + // Ambient + Text { + text: " Ambient" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + Layout.row: 2 + Layout.column: 1 + } + // Ambient color dialog + Button { + id: ambientButton + Layout.row: 2 + Layout.column: 2 + ToolTip.text: "Open color dialog" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + background: Rectangle { + implicitWidth: 40 + implicitHeight: 40 + radius: 5 + border.color: getButtonColor(0, false) + border.width: 2 + color: getButtonColor(0, true) + } + onClicked: { + sendMaterialColor("ambient", getButtonColor(0, true)) + } + } + // Ambient red + Item { + Layout.row: 2 + Layout.column: 3 + Layout.fillWidth: true + height: 40 + Loader { + id: rAmbientLoader + anchors.fill: parent + property double numberValue: model.data[0] + sourceComponent: spinBoxMaterialColor + onLoaded: { + rAmbientItem = rAmbientLoader.item + } + } + } + // Ambient green + Item { + Layout.row: 2 + Layout.column: 4 + Layout.fillWidth: true + height: 40 + Loader { + id: gAmbientLoader + anchors.fill: parent + property double numberValue: model.data[1] + sourceComponent: spinBoxMaterialColor + onLoaded: { + gAmbientItem = gAmbientLoader.item + } + } + } + // Ambient blue + Item { + Layout.row: 2 + Layout.column: 5 + Layout.fillWidth: true + height: 40 + Loader { + id: bAmbientLoader + anchors.fill: parent + property double numberValue: model.data[2] + sourceComponent: spinBoxMaterialColor + onLoaded: { + bAmbientItem = bAmbientLoader.item + } + } + } + // Ambient alpha + Item { + Layout.row: 2 + Layout.column: 6 + Layout.fillWidth: true + height: 40 + Loader { + id: aAmbientLoader + anchors.fill: parent + property double numberValue: model.data[3] + sourceComponent: spinBoxMaterialColor + onLoaded: { + aAmbientItem = aAmbientLoader.item + } + } + } // end Ambient + + // Diffuse + Text { + text: " Diffuse" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + Layout.row: 3 + Layout.column: 1 + } + // Diffuse color dialog + Button { + id: diffuseButton + Layout.row: 3 + Layout.column: 2 + ToolTip.text: "Open color dialog" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + background: Rectangle { + implicitWidth: 40 + implicitHeight: 40 + radius: 5 + border.color: getButtonColor(4, false) + border.width: 2 + color: getButtonColor(4, true) + } + onClicked: { + sendMaterialColor("diffuse", getButtonColor(4, true)) + } + } + // Diffuse red + Item { + Layout.row: 3 + Layout.column: 3 + Layout.fillWidth: true + height: 40 + Loader { + id: rDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[4] + sourceComponent: spinBoxMaterialColor + onLoaded: { + rDiffuseItem = rDiffuseLoader.item + } + } + } + // Diffuse green + Item { + Layout.row: 3 + Layout.column: 4 + Layout.fillWidth: true + height: 40 + Loader { + id: gDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[5] + sourceComponent: spinBoxMaterialColor + onLoaded: { + gDiffuseItem = gDiffuseLoader.item + } + } + } + // Diffuse blue + Item { + Layout.row: 3 + Layout.column: 5 + Layout.fillWidth: true + height: 40 + Loader { + id: bDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[6] + sourceComponent: spinBoxMaterialColor + onLoaded: { + bDiffuseItem = bDiffuseLoader.item + } + } + } + // Diffuse alpha + Item { + Layout.row: 3 + Layout.column: 6 + Layout.fillWidth: true + height: 40 + Loader { + id: aDiffuseLoader + anchors.fill: parent + property double numberValue: model.data[7] + sourceComponent: spinBoxMaterialColor + onLoaded: { + aDiffuseItem = aDiffuseLoader.item + } + } + } // end Diffuse + + // Specular + Text { + text: " Specular" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + Layout.row: 4 + Layout.column: 1 + } + // Specular color dialog + Button { + id: specularButton + Layout.row: 4 + Layout.column: 2 + ToolTip.text: "Open color dialog" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + background: Rectangle { + implicitWidth: 40 + implicitHeight: 40 + radius: 5 + border.color: getButtonColor(8, false) + border.width: 2 + color: getButtonColor(8, true) + } + onClicked: { + sendMaterialColor("specular", getButtonColor(8, true)) + } + } + // Specular red + Item { + Layout.row: 4 + Layout.column: 3 + Layout.fillWidth: true + height: 40 + Loader { + id: rSpecularLoader + anchors.fill: parent + property double numberValue: model.data[8] + sourceComponent: spinBoxMaterialColor + onLoaded: { + rSpecularItem = rSpecularLoader.item + } + } + } + // Specular green + Item { + Layout.row: 4 + Layout.column: 4 + Layout.fillWidth: true + height: 40 + Loader { + id: gSpecularLoader + anchors.fill: parent + property double numberValue: model.data[9] + sourceComponent: spinBoxMaterialColor + onLoaded: { + gSpecularItem = gSpecularLoader.item + } + } + } + // Specular blue + Item { + Layout.row: 4 + Layout.column: 5 + Layout.fillWidth: true + height: 40 + Loader { + id: bSpecularLoader + anchors.fill: parent + property double numberValue: model.data[10] + sourceComponent: spinBoxMaterialColor + onLoaded: { + bSpecularItem = bSpecularLoader.item + } + } + } + // Specular alpha + Item { + Layout.row: 4 + Layout.column: 6 + Layout.fillWidth: true + height: 40 + Loader { + id: aSpecularLoader + anchors.fill: parent + property double numberValue: model.data[11] + sourceComponent: spinBoxMaterialColor + onLoaded: { + aSpecularItem = aSpecularLoader.item + } + } + } // end Specular + + // Emissive + Text { + text: " Emissive" + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + Layout.row: 5 + Layout.column: 1 + } + // Emissive color dialog + Button { + id: emissiveButton + Layout.row: 5 + Layout.column: 2 + ToolTip.text: "Open color dialog" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + background: Rectangle { + implicitWidth: 40 + implicitHeight: 40 + radius: 5 + border.color: getButtonColor(12, false) + border.width: 2 + color: getButtonColor(12, true) + } + onClicked: { + sendMaterialColor("emissive", getButtonColor(12, true)) + } + } + // Emissive red + Item { + Layout.row: 5 + Layout.column: 3 + Layout.fillWidth: true + height: 40 + Loader { + id: rEmissiveLoader + anchors.fill: parent + property double numberValue: model.data[12] + sourceComponent: spinBoxMaterialColor + onLoaded: { + rEmissiveItem = rEmissiveLoader.item + } + } + } + // Emissive green + Item { + Layout.row: 5 + Layout.column: 4 + Layout.fillWidth: true + height: 40 + Loader { + id: gEmissiveLoader + anchors.fill: parent + property double numberValue: model.data[13] + sourceComponent: spinBoxMaterialColor + onLoaded: { + gEmissiveItem = gEmissiveLoader.item + } + } + } + // Emissive blue + Item { + Layout.row: 5 + Layout.column: 5 + Layout.fillWidth: true + height: 40 + Loader { + id: bEmissiveLoader + anchors.fill: parent + property double numberValue: model.data[14] + sourceComponent: spinBoxMaterialColor + onLoaded: { + bEmissiveItem = bEmissiveLoader.item + } + } + } + // Emissive alpha + Item { + Layout.row: 5 + Layout.column: 6 + Layout.fillWidth: true + height: 40 + Loader { + id: aEmissiveLoader + anchors.fill: parent + property double numberValue: model.data[15] + sourceComponent: spinBoxMaterialColor + onLoaded: { + aEmissiveItem = aEmissiveLoader.item + } + } + } // end Emissive + } // end GridLayout + } // end ColumnLayout (id: grid) + } // Rectangle (id: content) + } +} diff --git a/src/gui/plugins/component_inspector/Physics.qml b/src/gui/plugins/component_inspector/Physics.qml index 71f0a34fda5..936aba7cae8 100644 --- a/src/gui/plugins/component_inspector/Physics.qml +++ b/src/gui/plugins/component_inspector/Physics.qml @@ -116,48 +116,11 @@ Rectangle { Column { anchors.fill: parent - // Header - Rectangle { + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { 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" - } - } + // Using the default header text values. } // Content diff --git a/src/gui/plugins/component_inspector/Pose3d.qml b/src/gui/plugins/component_inspector/Pose3d.qml index 8d909e6c4cc..e9806463e41 100644 --- a/src/gui/plugins/component_inspector/Pose3d.qml +++ b/src/gui/plugins/component_inspector/Pose3d.qml @@ -93,7 +93,7 @@ Rectangle { value: writableSpin.activeFocus ? writableSpin.value : numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: 6 + decimals: getDecimals(writableSpin.width) onEditingFinished: { sendPose() } @@ -111,7 +111,7 @@ Rectangle { horizontalAlignment: Text.AlignRight verticalAlignment: Text.AlignVCenter text: { - var decimals = numberText.width < 100 ? 2 : 6 + var decimals = getDecimals(numberText.width) return numberValue.toFixed(decimals) } } @@ -120,48 +120,11 @@ Rectangle { Column { anchors.fill: parent - // Header - Rectangle { + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { 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" - } - } + // Using the default header text values. } // Content diff --git a/src/gui/plugins/component_inspector/SphericalCoordinates.qml b/src/gui/plugins/component_inspector/SphericalCoordinates.qml new file mode 100644 index 00000000000..4fe6cb74315 --- /dev/null +++ b/src/gui/plugins/component_inspector/SphericalCoordinates.qml @@ -0,0 +1,313 @@ +/* + * 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 spherical coordinates 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 + + // Icon size + property int iconWidth: 20 + property int iconHeight: 20 + + // Send new data to C++ + function sendSphericalCoordinates() { + // TODO(anyone) There's a loss of precision when these values get to C++ + componentInspector.onSphericalCoordinates( + surfaceText.text, + latSpin.value, + lonSpin.value, + elevationSpin.value, + headingSpin.value + ); + } + + 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 + + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { + id: header + // Using the default header text values. + } + + // 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: 4 + + // Left spacer + Item { + Layout.rowSpan: 5 + width: margin + indentation + } + + // Surface type + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: latText.width + indentation*3 + Rectangle { + color: "transparent" + id: surfaceSpacer + height: iconHeight + width: iconWidth + y: 10 + } + + Text { + text: 'Surface' + leftPadding: 12 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.left: surfaceSpacer.right + anchors.verticalCenter: parent.verticalCenter + } + } + Text { + id: surfaceText + text: model.data[0] + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: Material.theme == Material.Light ? "black" : "white" + font.pointSize: 12 + elide: Text.ElideLeft + } + + // Right spacer + Item { + Layout.rowSpan: 5 + width: margin + } + + // Latitude + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: latText.width + indentation*3 + Loader { + id: loaderPlotLat + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotLat.item.componentInfo = "latitude" + + Text { + id : latText + text: 'Latitude (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: latSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[1] + value: latSpin.activeFocus ? latSpin.value : numberValue + minimumValue: -90 + maximumValue: 90 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Longitude + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: lonText.width + indentation*3 + Loader { + id: loaderPlotLon + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotLon.item.componentInfo = "longitude" + + Text { + id : lonText + text: 'Longitude (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: lonSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[2] + value: lonSpin.activeFocus ? lonSpin.value : numberValue + minimumValue: -180 + maximumValue: 180 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Elevation + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: elevationText.width + indentation*3 + Loader { + id: loaderPlotElevation + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotElevation.item.componentInfo = "elevation" + + Text { + id : elevationText + text: 'Elevation (m)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: elevationSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[3] + value: elevationSpin.activeFocus ? elevationSpin.value : numberValue + minimumValue: -100000 + maximumValue: 100000 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + + // Heading + Rectangle { + color: "transparent" + height: 40 + Layout.preferredWidth: headingText.width + indentation*3 + Loader { + id: loaderPlotHeading + width: iconWidth + height: iconHeight + y:10 + sourceComponent: plotIcon + } + Component.onCompleted: loaderPlotHeading.item.componentInfo = "heading" + + Text { + id : headingText + text: 'Heading (°)' + leftPadding: 15 + color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + font.pointSize: 12 + anchors.centerIn: parent + } + } + IgnSpinBox { + id: headingSpin + Layout.fillWidth: true + height: 40 + property double numberValue: model.data[4] + value: headingSpin.activeFocus ? headingSpin.value : numberValue + minimumValue: -180 + maximumValue: 180 + decimals: 12 + stepSize: 0.1 + onEditingFinished: { + sendSphericalCoordinates() + } + } + } + } + } +} diff --git a/src/gui/plugins/component_inspector/Vector3d.qml b/src/gui/plugins/component_inspector/Vector3d.qml index 40e8129d23e..1cc4f42ca60 100644 --- a/src/gui/plugins/component_inspector/Vector3d.qml +++ b/src/gui/plugins/component_inspector/Vector3d.qml @@ -58,7 +58,7 @@ Rectangle { value: numberValue minimumValue: -spinMax maximumValue: spinMax - decimals: writableSpin.width < 100 ? 2 : 6 + decimals: getDecimals(writableSpin.width) } } @@ -73,7 +73,7 @@ Rectangle { horizontalAlignment: Text.AlignRight verticalAlignment: Text.AlignVCenter text: { - var decimals = numberText.width < 100 ? 2 : 6 + var decimals = getDecimals(numberText.width) return numberValue.toFixed(decimals) } } @@ -110,48 +110,11 @@ Rectangle { Column { anchors.fill: parent - // Header - Rectangle { + // The expanding header. Make sure that the content to expand has an id set + // to the value "content". + ExpandingTypeHeader { 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" - } - } + // Using the default header text values. } // Content diff --git a/src/gui/plugins/copy_paste/CMakeLists.txt b/src/gui/plugins/copy_paste/CMakeLists.txt new file mode 100644 index 00000000000..8eadfbeb2ad --- /dev/null +++ b/src/gui/plugins/copy_paste/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_gui_plugin(CopyPaste + SOURCES CopyPaste.cc + QT_HEADERS CopyPaste.hh +) diff --git a/src/gui/plugins/copy_paste/CopyPaste.cc b/src/gui/plugins/copy_paste/CopyPaste.cc new file mode 100644 index 00000000000..7b633c31767 --- /dev/null +++ b/src/gui/plugins/copy_paste/CopyPaste.cc @@ -0,0 +1,185 @@ +/* + * 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 "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + +#include "CopyPaste.hh" + +namespace ignition::gazebo +{ + class CopyPastePrivate + { + /// \brief The entity that is currently selected + public: Entity selectedEntity = kNullEntity; + + /// \brief The name of the entity that is currently selected + public: std::string selectedEntityName = ""; + + /// \brief The name of the entity that is copied + public: std::string copiedData = ""; + + /// \brief Transport node for handling service calls + public: transport::Node node; + + /// \brief Name of the copy service + public: const std::string copyService = "/gui/copy"; + + /// \brief Name of the paste service + public: const std::string pasteService = "/gui/paste"; + + /// \brief A mutex to ensure that there are no race conditions between + /// copy/paste + public: std::mutex mutex; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +CopyPaste::CopyPaste() + : GuiSystem(), dataPtr(std::make_unique()) +{ + if (!this->dataPtr->node.Advertise(this->dataPtr->copyService, + &CopyPaste::CopyServiceCB, this)) + { + ignerr << "Error advertising service [" << this->dataPtr->copyService + << "]" << std::endl; + } + + if (!this->dataPtr->node.Advertise(this->dataPtr->pasteService, + &CopyPaste::PasteServiceCB, this)) + { + ignerr << "Error advertising service [" << this->dataPtr->pasteService + << "]" << std::endl; + } +} + +///////////////////////////////////////////////// +CopyPaste::~CopyPaste() = default; + +///////////////////////////////////////////////// +void CopyPaste::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Copy/Paste"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); +} + +///////////////////////////////////////////////// +void CopyPaste::Update(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + std::lock_guard guard(this->dataPtr->mutex); + auto nameComp = + _ecm.Component(this->dataPtr->selectedEntity); + if (!nameComp) + return; + this->dataPtr->selectedEntityName = nameComp->Data(); +} + +///////////////////////////////////////////////// +void CopyPaste::OnCopy() +{ + std::lock_guard guard(this->dataPtr->mutex); + this->dataPtr->copiedData = this->dataPtr->selectedEntityName; +} + +///////////////////////////////////////////////// +void CopyPaste::OnPaste() +{ + std::lock_guard guard(this->dataPtr->mutex); + + // we should only paste if something has been copied + if (!this->dataPtr->copiedData.empty()) + { + ignition::gui::events::SpawnCloneFromName event(this->dataPtr->copiedData); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); + } +} + +///////////////////////////////////////////////// +bool CopyPaste::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gazebo::gui::events::EntitiesSelected::kType) + { + std::lock_guard guard(this->dataPtr->mutex); + + auto selectedEvent = + reinterpret_cast(_event); + if (selectedEvent && (selectedEvent->Data().size() == 1u)) + this->dataPtr->selectedEntity = selectedEvent->Data()[0]; + } + if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent && keyEvent->matches(QKeySequence::Copy)) + { + this->OnCopy(); + } + else if (keyEvent && keyEvent->matches(QKeySequence::Paste)) + { + this->OnPaste(); + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +bool CopyPaste::CopyServiceCB(const ignition::msgs::StringMsg &_req, + ignition::msgs::Boolean &_resp) +{ + { + std::lock_guard guard(this->dataPtr->mutex); + this->dataPtr->copiedData = _req.data(); + } + _resp.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool CopyPaste::PasteServiceCB(const ignition::msgs::Empty &/*_req*/, + ignition::msgs::Boolean &_resp) +{ + this->OnPaste(); + _resp.set_data(true); + return true; +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::CopyPaste, + ignition::gui::Plugin) diff --git a/src/gui/plugins/copy_paste/CopyPaste.hh b/src/gui/plugins/copy_paste/CopyPaste.hh new file mode 100644 index 00000000000..9b54d85b949 --- /dev/null +++ b/src/gui/plugins/copy_paste/CopyPaste.hh @@ -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. + * +*/ + +#ifndef IGNITION_GAZEBO_GUI_COPYPASTE_HH_ +#define IGNITION_GAZEBO_GUI_COPYPASTE_HH_ + +#include + +#include + +#include "ignition/gazebo/gui/GuiSystem.hh" + +namespace ignition +{ +namespace gazebo +{ + class CopyPastePrivate; + + /// \brief Plugin for copying/pasting entities in the GUI. + class CopyPaste : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: CopyPaste(); + + /// \brief Destructor + public: ~CopyPaste() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback to copy the selected entity + public slots: void OnCopy(); + + /// \brief Callback to paste the data that has been copied, if copied data + /// exists. + public slots: void OnPaste(); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Callback for handling a copy service request + /// \param[in] _req The service request, which contains the name of the + /// entity to be copied + /// \param[out] _resp The service response + /// \return Whether the service was successful (true) or not (false) + private: bool CopyServiceCB(const ignition::msgs::StringMsg &_req, + ignition::msgs::Boolean &_resp); + + /// \brief Callback for handling a paste service request + /// \param[in] _req The service request + /// \param[out] _resp The service response + /// \return Whether the service was successful (true) or not (false) + private: bool PasteServiceCB(const ignition::msgs::Empty &_req, + ignition::msgs::Boolean &_resp); + + /// \internal + /// \brief Pointer to private data + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/tape_measure/TapeMeasure.qml b/src/gui/plugins/copy_paste/CopyPaste.qml similarity index 58% rename from src/gui/plugins/tape_measure/TapeMeasure.qml rename to src/gui/plugins/copy_paste/CopyPaste.qml index 29362bcf090..84513cc2154 100644 --- a/src/gui/plugins/tape_measure/TapeMeasure.qml +++ b/src/gui/plugins/copy_paste/CopyPaste.qml @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. @@ -21,80 +21,51 @@ import QtQuick.Controls.Material 2.2 import QtQuick.Controls.Material.impl 2.2 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 -import "qrc:/qml" ToolBar { - id: tapeMeasure - Layout.minimumWidth: 250 + id: copyPaste + Layout.minimumWidth: 200 Layout.minimumHeight: 100 - property var distance: 0.0 - - function updateDistance() { - distance = TapeMeasure.Distance(); - } - - Connections { - target: TapeMeasure - onNewDistance: { - updateDistance(); - } - } - background: Rectangle { color: "transparent" } - ButtonGroup { - id: group - } - RowLayout { - spacing: 1 + spacing: 2 ToolButton { - id: select - checkable: true - checked: true - ButtonGroup.group: group - ToolTip.text: "Tape Measure Tool" + id: copy + ToolTip.text: "Copy" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval contentItem: Image { fillMode: Image.Pad horizontalAlignment: Image.AlignHCenter verticalAlignment: Image.AlignVCenter - source: "tape_measure.png" - sourceSize.width: 36; - sourceSize.height: 36; + source: "copy_object.png" + sourceSize.width: 24; + sourceSize.height: 24; } onClicked: { - TapeMeasure.OnMeasure(); + CopyPaste.OnCopy() } } - ToolButton { - id: reset - checkable: true - checked: true - ButtonGroup.group: group - ToolTip.text: "Reset measurement" + ToolButton{ + id: paste + ToolTip.text: "Paste" ToolTip.visible: hovered ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval contentItem: Image { fillMode: Image.Pad horizontalAlignment: Image.AlignHCenter verticalAlignment: Image.AlignVCenter - source: "trashcan.png" - sourceSize.width: 36; - sourceSize.height: 36; + source: "paste_object.png" + sourceSize.width: 24; + sourceSize.height: 24; } onClicked: { - TapeMeasure.OnReset(); + CopyPaste.OnPaste() } } - Text { - text: qsTr(" Distance (m): " + distance.toFixed(3)) - font.pointSize: 14 - color: Material.theme == Material.Light ? "#444444" : "#bbbbbb" - } } } diff --git a/src/gui/plugins/copy_paste/CopyPaste.qrc b/src/gui/plugins/copy_paste/CopyPaste.qrc new file mode 100644 index 00000000000..07e6a4e1fa1 --- /dev/null +++ b/src/gui/plugins/copy_paste/CopyPaste.qrc @@ -0,0 +1,8 @@ + + + + CopyPaste.qml + copy_object.png + paste_object.png + + diff --git a/src/gui/plugins/copy_paste/copy_object.png b/src/gui/plugins/copy_paste/copy_object.png new file mode 100644 index 00000000000..ccda68f882f Binary files /dev/null and b/src/gui/plugins/copy_paste/copy_object.png differ diff --git a/src/gui/plugins/copy_paste/paste_object.png b/src/gui/plugins/copy_paste/paste_object.png new file mode 100644 index 00000000000..3f31523082b Binary files /dev/null and b/src/gui/plugins/copy_paste/paste_object.png differ diff --git a/src/gui/plugins/entity_context_menu/CMakeLists.txt b/src/gui/plugins/entity_context_menu/CMakeLists.txt new file mode 100644 index 00000000000..85dddd702a6 --- /dev/null +++ b/src/gui/plugins/entity_context_menu/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(EntityContextMenuPlugin + SOURCES + EntityContextMenuPlugin.cc + QT_HEADERS + EntityContextMenuPlugin.hh + PUBLIC_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc new file mode 100644 index 00000000000..a1507b884c6 --- /dev/null +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.cc @@ -0,0 +1,208 @@ +/* + * 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 "EntityContextMenuPlugin.hh" + +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +namespace ignition::gazebo +{ + class EntityContextMenuPrivate + { + /// \brief Perform operations in the render thread. + public: void OnRender(); + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Entity context menu handler + public: EntityContextMenuHandler entityContextMenuHandler; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +void EntityContextMenuPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + + igndbg << "Entity context menu plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + } +} + +///////////////////////////////////////////////// +EntityContextMenu::EntityContextMenu() + : gui::Plugin(), dataPtr(std::make_unique()) +{ + qmlRegisterType( + "RenderWindowOverlay", 1, 0, "RenderWindowOverlay"); +} + +EntityContextMenu::~EntityContextMenu() = default; + +///////////////////////////////////////////////// +void EntityContextMenu::LoadConfig(const tinyxml2::XMLElement *) +{ + EntityContextMenuItem *renderWindowOverlay = + this->PluginItem()->findChild(); + if (!renderWindowOverlay) + { + ignerr << "Unable to find Render Window Overlay item. " + << "Render window overlay will not be created" << std::endl; + return; + } + + renderWindowOverlay->SetEntityContextMenuHandler( + this->dataPtr->entityContextMenuHandler); + + if (this->title.empty()) + this->title = "EntityContextMenu"; + + ignition::gui::App()->findChild + ()->installEventFilter(this); +} + +//////////////////////////////////////////////// +bool EntityContextMenu::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + else if (_event->type() == ignition::gui::events::RightClickOnScene::kType) + { + ignition::gui::events::RightClickOnScene *_e = + static_cast(_event); + if (_e) + { + this->dataPtr->entityContextMenuHandler.HandleMouseContextMenu( + _e->Mouse(), this->dataPtr->camera); + } + } + + return QObject::eventFilter(_obj, _event); +} + + +///////////////////////////////////////////////// +EntityContextMenuItem::EntityContextMenuItem(QQuickItem *_parent) + : QQuickItem(_parent) +{ + this->setAcceptedMouseButtons(Qt::AllButtons); + this->setFlag(ItemHasContents); +} + +///////////////////////////////////////////////// +void EntityContextMenuItem::SetEntityContextMenuHandler( + const EntityContextMenuHandler &_entityContextMenuHandler) +{ + this->connect( + &_entityContextMenuHandler, + &EntityContextMenuHandler::ContextMenuRequested, + this, + &EntityContextMenuItem::OnContextMenuRequested, + Qt::QueuedConnection); +} + +/////////////////////////////////////////////////// +void EntityContextMenuItem::OnContextMenuRequested( + QString _entity, int _mouseX, int _mouseY) +{ + emit openContextMenu(std::move(_entity), _mouseX, _mouseY); +} + +///////////////////////////////////////////////// +EntityContextMenuHandler::EntityContextMenuHandler() +{ +} + +void EntityContextMenuHandler::HandleMouseContextMenu( + const common::MouseEvent &_mouseEvent, const rendering::CameraPtr &_camera) +{ + if (!_mouseEvent.Dragging() && + _mouseEvent.Type() == common::MouseEvent::RELEASE && + _mouseEvent.Button() == common::MouseEvent::RIGHT) + { + math::Vector2i dt = + _mouseEvent.PressPos() - _mouseEvent.Pos(); + + // check for click with some tol for mouse movement + if (dt.Length() > 5.0) + return; + + rendering::VisualPtr visual = _camera->Scene()->VisualAt( + _camera, + _mouseEvent.Pos()); + + if (!visual) + return; + + // get model visual + while (visual->HasParent() && visual->Parent() != + visual->Scene()->RootVisual()) + { + visual = std::dynamic_pointer_cast(visual->Parent()); + } + + emit ContextMenuRequested( + visual->Name().c_str(), _mouseEvent.Pos().X(), _mouseEvent.Pos().Y()); + } +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::EntityContextMenu, + ignition::gui::Plugin) diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.hh b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.hh new file mode 100644 index 00000000000..4ed5858f3a4 --- /dev/null +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.hh @@ -0,0 +1,111 @@ +/* + * 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_GUI_PLUGINS_ENTITY_CONTEXT_MENU_HH_ +#define IGNITION_GUI_PLUGINS_ENTITY_CONTEXT_MENU_HH_ + +#include + +#include + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ + class EntityContextMenuPrivate; + + /// \brief This plugin is in charge of showing the entity context menu when + /// the right button is clicked on a visual. + class EntityContextMenu : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: EntityContextMenu(); + + /// \brief Destructor + public: ~EntityContextMenu() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; + + class EntityContextMenuHandler : public QObject + { + Q_OBJECT + + /// \brief Constructor + public: EntityContextMenuHandler(); + + /// \brief Handle mouse event for context menu + /// \param[in] _mouseEvent Right click mouse event + /// \param[in] _camera User camera + public: void HandleMouseContextMenu(const common::MouseEvent &_mouseEvent, + const rendering::CameraPtr &_camera); + + /// \brief Signal fired when context menu event is triggered + /// \param[in] _entity Scoped name of entity. + /// \param[in] _mouseX X coordinate of the right click + /// \param[in] _mouseY Y coordinate of the right click + signals: void ContextMenuRequested( + QString _entity, int _mouseX, int _mouseY); + }; + + /// \brief A QQUickItem that manages the render window + class EntityContextMenuItem : public QQuickItem + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _parent Parent item + public: explicit EntityContextMenuItem(QQuickItem *_parent = nullptr); + + /// \brief Set the entity context menu handler + /// \param[in] _entityContextMenuHandler Entity context menu handler + public: void SetEntityContextMenuHandler( + const EntityContextMenuHandler &_entityContextMenuHandler); + + /// \brief Signal fired to open context menu + /// Note that the function name needs to start with lowercase in order for + /// the connection to work on the QML side + /// \param[in] _entity Scoped name of entity. + /// \param[in] _mouseX X coordinate of the right click + /// \param[in] _mouseY Y coordinate of the right click + signals: void openContextMenu(QString _entity, int _mouseX, int _mouseY); // NOLINT + + /// \brief Qt callback when context menu request is received + /// \param[in] _entity Scoped name of entity. + /// \param[in] _mouseX X coordinate of the right click + /// \param[in] _mouseY Y coordinate of the right click + public slots: void OnContextMenuRequested( + QString _entity, int _mouseX, int _mouseY); + }; +} +} + +#endif diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml new file mode 100644 index 00000000000..66e2f56d286 --- /dev/null +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qml @@ -0,0 +1,47 @@ +/* + * 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.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 +import RenderWindowOverlay 1.0 +import IgnGazebo 1.0 as IgnGazebo + +Rectangle { + visible: false + color: "transparent" + + RenderWindowOverlay { + id: renderWindowOverlay + objectName: "renderWindowOverlay" + anchors.fill: parent + + Connections { + target: renderWindowOverlay + onOpenContextMenu: + { + entityContextMenu.open(_entity, "model", + _mouseX, _mouseY); + } + } + } + + IgnGazebo.EntityContextMenu { + id: entityContextMenu + anchors.fill: parent + } +} diff --git a/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qrc b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qrc new file mode 100644 index 00000000000..96bffedd5e8 --- /dev/null +++ b/src/gui/plugins/entity_context_menu/EntityContextMenuPlugin.qrc @@ -0,0 +1,5 @@ + + + EntityContextMenuPlugin.qml + + diff --git a/src/gui/plugins/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 79de5dfb7f2..a1eb39a2e12 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -17,12 +17,18 @@ #include "EntityTree.hh" +#include #include +#include +#include +#include #include #include +#include #include #include +#include #include #include @@ -39,9 +45,11 @@ #include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" -#include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Primitives.hh" + namespace ignition::gazebo { class EntityTreePrivate @@ -54,6 +62,16 @@ namespace ignition::gazebo /// \brief World entity public: Entity worldEntity{kNullEntity}; + + /// \brief List of new entities from a gui event + public: std::set newEntities; + + /// \brief List of removed entities from a gui event + public: std::set removedEntities; + + /// \brief Mutex to protect gui event and system upate call race conditions + /// for newEntities and removedEntities + public: std::mutex newRemovedEntityMutex; }; } @@ -100,11 +118,12 @@ QString entityType(Entity _entity, ///////////////////////////////////////////////// TreeModel::TreeModel() : QStandardItemModel() { + qRegisterMetaType("Entity"); } ///////////////////////////////////////////////// -void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, - unsigned int _parentEntity, const QString &_type) +void TreeModel::AddEntity(Entity _entity, const QString &_entityName, + Entity _parentEntity, const QString &_type) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_PROFILE("TreeModel::AddEntity"); @@ -165,7 +184,7 @@ void TreeModel::AddEntity(unsigned int _entity, const QString &_entityName, } ///////////////////////////////////////////////// -void TreeModel::RemoveEntity(unsigned int _entity) +void TreeModel::RemoveEntity(Entity _entity) { IGN_PROFILE("TreeModel::RemoveEntity"); QStandardItem *item{nullptr}; @@ -247,7 +266,7 @@ QString TreeModel::ScopedName(const QModelIndex &_index) const } ///////////////////////////////////////////////// -unsigned int TreeModel::EntityId(const QModelIndex &_index) const +Entity TreeModel::EntityId(const QModelIndex &_index) const { Entity entity{kNullEntity}; QStandardItem *item = this->itemFromIndex(_index); @@ -329,9 +348,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), + Q_ARG(Entity, _entity), Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), + Q_ARG(Entity, parentEntity), Q_ARG(QString, entityType(_entity, _ecm))); return true; }); @@ -359,9 +378,9 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity), + Q_ARG(Entity, _entity), Q_ARG(QString, QString::fromStdString(_name->Data())), - Q_ARG(unsigned int, parentEntity), + Q_ARG(Entity, parentEntity), Q_ARG(QString, entityType(_entity, _ecm))); return true; }); @@ -373,13 +392,62 @@ void EntityTree::Update(const UpdateInfo &, EntityComponentManager &_ecm) { QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", Qt::QueuedConnection, - Q_ARG(unsigned int, _entity)); + Q_ARG(Entity, _entity)); return true; }); + + { + // update the entity tree with new/removed entities from gui events + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + + for (auto entity : this->dataPtr->newEntities) + { + // make sure the entity to be added has a name and parent + auto nameComp = _ecm.Component(entity); + if (!nameComp) + { + ignerr << "Could not add entity [" << entity << "] to the entity tree " + << "because it does not have a name component.\n"; + continue; + } + auto parentComp = _ecm.Component(entity); + if (!parentComp) + { + ignerr << "Could not add entity [" << entity << "] to the entity tree " + << "because it does not have a parent entity component.\n"; + continue; + } + + // World children are top-level + auto parentEntity = parentComp->Data(); + if (this->dataPtr->worldEntity != kNullEntity && + parentEntity == this->dataPtr->worldEntity) + { + parentEntity = kNullEntity; + } + + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "AddEntity", + Qt::QueuedConnection, + Q_ARG(Entity, entity), + Q_ARG(QString, QString::fromStdString(nameComp->Data())), + Q_ARG(Entity, parentEntity), + Q_ARG(QString, entityType(entity, _ecm))); + } + + for (auto entity : this->dataPtr->removedEntities) + { + QMetaObject::invokeMethod(&this->dataPtr->treeModel, "RemoveEntity", + Qt::QueuedConnection, + Q_ARG(Entity, entity)); + } + + this->dataPtr->newEntities.clear(); + this->dataPtr->removedEntities.clear(); + } } ///////////////////////////////////////////////// -void EntityTree::OnEntitySelectedFromQml(unsigned int _entity) +void EntityTree::OnEntitySelectedFromQml(Entity _entity) { std::vector entitySet {_entity}; gui::events::EntitiesSelected event(entitySet, true); @@ -397,6 +465,65 @@ void EntityTree::DeselectAllEntities() &event); } +///////////////////////////////////////////////// +void EntityTree::OnInsertEntity(const QString &_type) +{ + std::string modelSdfString = getPrimitive(_type.toStdString()); + ignition::gui::events::SpawnFromDescription event(modelSdfString); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); +} + +///////////////////////////////////////////////// +void EntityTree::OnLoadMesh(const QString &_mesh) +{ + std::string meshStr = _mesh.toStdString(); + if (QUrl(_mesh).isLocalFile()) + { + // mesh to sdf model + common::rtrim(meshStr); + + if (!common::MeshManager::Instance()->IsValidFilename(meshStr)) + { + QString errTxt = QString::fromStdString("Invalid URI: " + meshStr + + "\nOnly mesh file types DAE, OBJ, and STL are supported."); + return; + } + + std::string filename = common::basename(meshStr); + std::vector splitName = common::split(filename, "."); + + std::string sdf = "" + "" + "" + "" + "" + "" + "" + "" + meshStr + "" + "" + "" + "" + "" + "" + "" + "" + meshStr + "" + "" + "" + "" + "" + "" + ""; + + ignition::gui::events::SpawnFromDescription event(sdf); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); + + } +} + ///////////////////////////////////////////////// bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) { @@ -413,7 +540,7 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) QMetaObject::invokeMethod(this->PluginItem(), "onEntitySelectedFromCpp", Qt::QueuedConnection, Q_ARG(QVariant, - QVariant(static_cast(entity)))); + QVariant(static_cast(entity)))); } } } @@ -428,6 +555,23 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) Qt::QueuedConnection); } } + else if (_event->type() == + ignition::gazebo::gui::events::GuiNewRemovedEntities::kType) + { + std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); + auto addedRemovedEvent = + reinterpret_cast(_event); + if (addedRemovedEvent) + { + // TODO(chapulina) Make these entities visually different from entities + // created from the server. + for (auto entity : addedRemovedEvent->NewEntities()) + this->dataPtr->newEntities.insert(entity); + + for (auto entity : addedRemovedEvent->RemovedEntities()) + this->dataPtr->removedEntities.insert(entity); + } + } // Standard event processing return QObject::eventFilter(_obj, _event); diff --git a/src/gui/plugins/entity_tree/EntityTree.hh b/src/gui/plugins/entity_tree/EntityTree.hh index 4f882de9f73..b899c129802 100644 --- a/src/gui/plugins/entity_tree/EntityTree.hh +++ b/src/gui/plugins/entity_tree/EntityTree.hh @@ -22,6 +22,7 @@ #include #include +#include #include namespace ignition @@ -50,14 +51,14 @@ namespace gazebo /// \param[in] _parentEntity Parent entity. By default, kNullEntity, which /// means it's a root entity. /// \param[in] _type Entity type - public slots: void AddEntity(unsigned int _entity, + public slots: void AddEntity(Entity _entity, const QString &_entityName, - unsigned int _parentEntity = kNullEntity, + Entity _parentEntity = kNullEntity, const QString &_type = QString()); /// \brief Remove an entity from the tree. /// \param[in] _entity Entity to be removed - public slots: void RemoveEntity(unsigned int _entity); + public slots: void RemoveEntity(Entity _entity); /// \brief Get the entity type of a tree item at specified index /// \param[in] _index Model index @@ -72,7 +73,7 @@ namespace gazebo /// \brief Get the entity ID of a tree item at specified index /// \param[in] _index Model index /// \return Entity ID - public: Q_INVOKABLE unsigned int EntityId(const QModelIndex &_index) const; + public: Q_INVOKABLE Entity EntityId(const QModelIndex &_index) const; /// \brief Keep track of which item corresponds to which entity. private: std::map entityItems; @@ -82,14 +83,14 @@ namespace gazebo { /// \brief Entity ID // cppcheck-suppress unusedStructMember - unsigned int entity; + Entity entity; /// \brief Entity name QString name; /// \brief Parent ID // cppcheck-suppress unusedStructMember - unsigned int parentEntity; + Entity parentEntity; /// \brief Entity type QString type; @@ -123,12 +124,20 @@ namespace gazebo /// \brief Callback when an entity has been selected. This should be /// called from QML. /// \param[in] _entity Entity being selected. - public: Q_INVOKABLE void OnEntitySelectedFromQml(unsigned int _entity); + public: Q_INVOKABLE void OnEntitySelectedFromQml(Entity _entity); /// \brief Callback when all entities have been deselected. /// This should be called from QML. public: Q_INVOKABLE void DeselectAllEntities(); + /// \brief Callback to insert a new entity + /// \param[in] _type Type of entity to insert + public: Q_INVOKABLE void OnInsertEntity(const QString &_type); + + /// \brief Callback to insert a new entity + /// \param[in] _type Type of entity to insert + public: Q_INVOKABLE void OnLoadMesh(const QString &_type); + // Documentation inherited protected: bool eventFilter(QObject *_obj, QEvent *_event) override; diff --git a/src/gui/plugins/entity_tree/EntityTree.qml b/src/gui/plugins/entity_tree/EntityTree.qml index 5e73519718c..b0cf0d55a32 100644 --- a/src/gui/plugins/entity_tree/EntityTree.qml +++ b/src/gui/plugins/entity_tree/EntityTree.qml @@ -21,12 +21,13 @@ import QtQuick.Controls 2.2 import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 +import QtQuick.Dialogs 1.0 import IgnGazebo 1.0 as IgnGazebo Rectangle { id: entityTree - color: "transparent" - Layout.minimumWidth: 250 + color: lightGrey + Layout.minimumWidth: 400 Layout.minimumHeight: 375 anchors.fill: parent @@ -35,6 +36,20 @@ Rectangle { */ property int tooltipDelay: 500 + /** + * Dark grey according to theme + */ + property color darkGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade200) : + Material.color(Material.Grey, Material.Shade900) + + /** + * Light grey according to theme + */ + property color lightGrey: (Material.theme == Material.Light) ? + Material.color(Material.Grey, Material.Shade100) : + Material.color(Material.Grey, Material.Shade800) + /** * Height of each item in pixels */ @@ -86,9 +101,160 @@ Rectangle { } } + Rectangle { + id: header + visible: true + height: addEntity.height + anchors.top: parent.top + anchors.left: parent.left + anchors.right: parent.right + width: parent.width + color: darkGrey + + RowLayout { + anchors.fill: parent + spacing: 0 + + Label { + text: "Entity Tree" + font.capitalization: Font.Capitalize + color: Material.theme == Material.Light ? "#444444" : "#cccccc" + font.pointSize: 12 + padding: 5 + } + + ToolButton { + anchors.right: parent.right + id: addEntity + ToolTip.text: "Add Entity" + ToolTip.visible: hovered + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "qrc:/Gazebo/images/plus.png" + sourceSize.width: 18; + sourceSize.height: 18; + } + onClicked: addEntityMenu.open() + + FileDialog { + id: loadFileDialog + title: "Load mesh" + folder: shortcuts.home + nameFilters: [ "Collada files (*.dae)", "(*.stl)", "(*.obj)" ] + selectMultiple: false + selectExisting: true + onAccepted: { + EntityTree.OnLoadMesh(fileUrl) + } + } + + Menu { + id: addEntityMenu + + MenuItem + { + id: box + text: "Box" + onClicked: { + EntityTree.OnInsertEntity("box") + } + } + + MenuItem + { + id: capsule + text: "Capsule" + onClicked: { + EntityTree.OnInsertEntity("capsule") + } + } + + MenuItem + { + id: cylinder + text: "Cylinder" + onClicked: { + EntityTree.OnInsertEntity("cylinder") + } + } + + MenuItem + { + id: ellipsoid + text: "Ellipsoid" + onClicked: { + EntityTree.OnInsertEntity("ellipsoid") + } + } + + MenuItem + { + id: sphere + text: "Sphere" + onClicked: { + EntityTree.OnInsertEntity("sphere") + } + } + + MenuItem + { + id: mesh + text: "Mesh" + onClicked: { + loadFileDialog.open() + } + } + + MenuSeparator { + padding: 0 + topPadding: 12 + bottomPadding: 12 + contentItem: Rectangle { + implicitWidth: 200 + implicitHeight: 1 + color: "#1E000000" + } + } + + MenuItem + { + id: directionalLight + text: "Directional" + onClicked: { + EntityTree.OnInsertEntity("directional") + } + } + + MenuItem + { + id: pointLight + text: "Point" + onClicked: { + EntityTree.OnInsertEntity("point") + } + } + + MenuItem + { + id: spotLight + text: "Spot" + onClicked: { + EntityTree.OnInsertEntity("spot") + } + } + } + } + } + } + TreeView { id: tree - anchors.fill: parent + anchors.top: header.bottom + anchors.bottom: parent.bottom + anchors.left: parent.left + anchors.right: parent.right model: EntityTreeModel selectionMode: SelectionMode.MultiSelection @@ -105,16 +271,20 @@ Rectangle { } style: TreeViewStyle { + frame: Rectangle { + border{ + color: lightGrey + } + } indentation: itemHeight * 0.75 headerDelegate: Rectangle { visible: false } - branchDelegate: Rectangle { height: itemHeight width: itemHeight * 0.75 - color: "transparent" + color: lightGrey Image { id: icon sourceSize.height: itemHeight * 0.4 diff --git a/src/gui/plugins/grid_config/CMakeLists.txt b/src/gui/plugins/grid_config/CMakeLists.txt deleted file mode 100644 index fed08c8844b..00000000000 --- a/src/gui/plugins/grid_config/CMakeLists.txt +++ /dev/null @@ -1,8 +0,0 @@ -gz_add_gui_plugin(GridConfig - SOURCES - GridConfig.cc - QT_HEADERS - GridConfig.hh - PUBLIC_LINK_LIBS - ${IGNITION-RENDERING_LIBRARIES} -) diff --git a/src/gui/plugins/grid_config/GridConfig.cc b/src/gui/plugins/grid_config/GridConfig.cc deleted file mode 100644 index 9dc9184e15a..00000000000 --- a/src/gui/plugins/grid_config/GridConfig.cc +++ /dev/null @@ -1,259 +0,0 @@ -/* - * 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 "GridConfig.hh" - -namespace ignition::gazebo -{ - struct GridParam - { - /// \brief Default horizontal cell count - int hCellCount{20}; - - /// \brief Default vertical cell count - int vCellCount{0}; - - /// \brief Default cell length - double cellLength{1.0}; - - /// \brief Default pose of grid - math::Pose3d pose{math::Pose3d::Zero}; - - /// \brief Default color of grid - math::Color color{math::Color(0.7f, 0.7f, 0.7f, 1.0f)}; - - /// \brief Default visible state - bool visible{true}; - }; - - class GridConfigPrivate - { - /// \brief Assume only one gridptr in a scene - public: rendering::GridPtr grid; - - /// \brief Default grid parameters - public: GridParam gridParam; - - /// \brief Flag that indicates whether there are new updates to be rendered. - public: bool dirty{false}; - }; -} - -using namespace ignition; -using namespace gazebo; - -///////////////////////////////////////////////// -GridConfig::GridConfig() - : ignition::gui::Plugin(), dataPtr(std::make_unique()) -{ -} - -///////////////////////////////////////////////// -GridConfig::~GridConfig() = default; - -///////////////////////////////////////////////// -void GridConfig::LoadConfig(const tinyxml2::XMLElement *) -{ - if (this->title.empty()) - this->title = "Grid config"; - - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->installEventFilter(this); -} - -///////////////////////////////////////////////// -bool GridConfig::eventFilter(QObject *_obj, QEvent *_event) -{ - if (_event->type() == ignition::gui::events::Render::kType) - { - // This event is called in Scene3d's RenderThread, so it's safe to make - // rendering calls here - this->UpdateGrid(); - } - - // Standard event processing - return QObject::eventFilter(_obj, _event); -} - -///////////////////////////////////////////////// -void GridConfig::UpdateGrid() -{ - // Load grid if it doesn't already exist - if (!this->dataPtr->grid) - this->LoadGrid(); - - // If grid was not loaded successfully, don't update - if (!this->dataPtr->grid) - return; - - if (!this->dataPtr->dirty) - return; - - this->dataPtr->grid->SetVerticalCellCount( - this->dataPtr->gridParam.vCellCount); - this->dataPtr->grid->SetCellCount( - this->dataPtr->gridParam.hCellCount); - this->dataPtr->grid->SetCellLength( - this->dataPtr->gridParam.cellLength); - - auto visual = this->dataPtr->grid->Parent(); - if (visual) - { - visual->SetLocalPose(this->dataPtr->gridParam.pose); - - auto mat = visual->Material(); - if (mat) - { - mat->SetAmbient(this->dataPtr->gridParam.color); - mat->SetDiffuse(this->dataPtr->gridParam.color); - mat->SetSpecular(this->dataPtr->gridParam.color); - } - else - { - ignerr << "Grid visual missing material" << std::endl; - } - - visual->SetVisible(this->dataPtr->gridParam.visible); - } - else - { - ignerr << "Grid missing parent visual" << std::endl; - } - - this->dataPtr->dirty = false; -} - -///////////////////////////////////////////////// -void GridConfig::LoadGrid() -{ - auto scene = rendering::sceneFromFirstRenderEngine(); - - // load grid - // if gridPtr found, load the existing gridPtr to class - for (unsigned int i = 0; i < scene->VisualCount(); ++i) - { - auto vis = scene->VisualByIndex(i); - if (!vis || vis->GeometryCount() == 0) - continue; - for (unsigned int j = 0; j < vis->GeometryCount(); ++j) - { - auto grid = std::dynamic_pointer_cast( - vis->GeometryByIndex(j)); - if (grid) - { - igndbg << "Attaching to existing grid" << std::endl; - this->dataPtr->grid = grid; - return; - } - } - } - - if (this->dataPtr->grid) - return; - - // Create grid - igndbg << "Creating grid" << std::endl; - - auto root = scene->RootVisual(); - this->dataPtr->grid = scene->CreateGrid(); - if (!this->dataPtr->grid) - { - ignwarn << "Failed to create grid, grid config plugin won't work." - << std::endl; - - // If we get here, most likely the render engine and scene are fully loaded, - // but they don't support grids. So stop trying. - ignition::gui::App()->findChild< - ignition::gui::MainWindow *>()->removeEventFilter(this); - return; - } - - this->dataPtr->grid->SetCellCount( - this->dataPtr->gridParam.hCellCount); - this->dataPtr->grid->SetVerticalCellCount( - this->dataPtr->gridParam.vCellCount); - this->dataPtr->grid->SetCellLength( - this->dataPtr->gridParam.cellLength); - - auto vis = scene->CreateVisual(); - root->AddChild(vis); - vis->SetLocalPose(this->dataPtr->gridParam.pose); - vis->AddGeometry(this->dataPtr->grid); - - auto mat = scene->CreateMaterial(); - mat->SetAmbient(this->dataPtr->gridParam.color); - mat->SetDiffuse(this->dataPtr->gridParam.color); - mat->SetSpecular(this->dataPtr->gridParam.color); - vis->SetMaterial(mat); -} - -///////////////////////////////////////////////// -void GridConfig::UpdateVCellCount(int _cellCount) -{ - this->dataPtr->gridParam.vCellCount = _cellCount; - this->dataPtr->dirty = true; -} - -///////////////////////////////////////////////// -void GridConfig::UpdateHCellCount(int _cellCount) -{ - this->dataPtr->gridParam.hCellCount = _cellCount; - this->dataPtr->dirty = true; -} - -///////////////////////////////////////////////// -void GridConfig::UpdateCellLength(double _length) -{ - this->dataPtr->gridParam.cellLength = _length; - this->dataPtr->dirty = true; -} - -///////////////////////////////////////////////// -void GridConfig::SetPose( - double _x, double _y, double _z, - double _roll, double _pitch, double _yaw) -{ - this->dataPtr->gridParam.pose = math::Pose3d(_x, _y, _z, _roll, _pitch, _yaw); - this->dataPtr->dirty = true; -} - -///////////////////////////////////////////////// -void GridConfig::SetColor(double _r, double _g, double _b, double _a) -{ - this->dataPtr->gridParam.color = math::Color(_r, _g, _b, _a); - this->dataPtr->dirty = true; -} - -///////////////////////////////////////////////// -void GridConfig::OnShow(bool _checked) -{ - this->dataPtr->gridParam.visible = _checked; - this->dataPtr->dirty = true; -} - -// Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::GridConfig, - ignition::gui::Plugin) diff --git a/src/gui/plugins/grid_config/GridConfig.hh b/src/gui/plugins/grid_config/GridConfig.hh deleted file mode 100644 index c39184e7e63..00000000000 --- a/src/gui/plugins/grid_config/GridConfig.hh +++ /dev/null @@ -1,88 +0,0 @@ -/* - * 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_GUI_GRIDCONFIG_HH_ -#define IGNITION_GAZEBO_GUI_GRIDCONFIG_HH_ - -#include - -#include -#include - -namespace ignition -{ -namespace gazebo -{ - class GridConfigPrivate; - - class GridConfig : public ignition::gui::Plugin - { - Q_OBJECT - - /// \brief Constructor - public: GridConfig(); - - /// \brief Destructor - public: ~GridConfig() override; - - // Documentation inherited - public: void LoadConfig(const tinyxml2::XMLElement *) override; - - // Documentation inherited - protected: bool eventFilter(QObject *_obj, QEvent *_event) override; - - /// \brief Update grid - public: void UpdateGrid(); - - /// \brief Callback to retrieve existing grid or create a new one. - /// \param[in] _scene Scene to look for grid. - public: void LoadGrid(); - - /// \brief Callback to update vertical cell count - /// \param[in] _cellCount new vertical cell count - public slots: void UpdateVCellCount(int _cellCount); - - /// \brief Callback to update horizontal cell count - /// \param[in] _cellCount new horizontal cell count - public slots: void UpdateHCellCount(int _cellCount); - - /// \brief Callback to update cell length - /// \param[in] _length new cell length - public slots: void UpdateCellLength(double _length); - - /// \brief Callback to update grid pose - /// \param[in] _x, _y, _z cartesion coordinates - /// \param[in] _roll, _pitch, _yaw principal coordinates - public slots: void SetPose(double _x, double _y, double _z, - double _roll, double _pitch, double _yaw); - - /// \brief Callback to update grid color - /// \param[in] _r, _g, _b, _a RGB color model with fourth alpha channel - public slots: void SetColor(double _r, double _g, double _b, double _a); - - /// \brief Callback when checkbox is clicked. - /// \param[in] _checked indicates show or hide grid - public slots: void OnShow(bool _checked); - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; -} -} - -#endif diff --git a/src/gui/plugins/grid_config/GridConfig.qml b/src/gui/plugins/grid_config/GridConfig.qml deleted file mode 100644 index d73efd47d8b..00000000000 --- a/src/gui/plugins/grid_config/GridConfig.qml +++ /dev/null @@ -1,325 +0,0 @@ -/* - * 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 2.1 -import QtQuick.Dialogs 1.0 -import QtQuick.Layouts 1.3 -import "qrc:/qml" - -GridLayout { - columns: 6 - columnSpacing: 10 - Layout.minimumWidth: 300 - Layout.minimumHeight: 625 - anchors.fill: parent - anchors.leftMargin: 10 - anchors.rightMargin: 10 - - // Left spacer - Item { - Layout.columnSpan: 1 - Layout.rowSpan: 15 - Layout.fillWidth: true - } - - CheckBox { - Layout.alignment: Qt.AlignHCenter - id: showgrid - Layout.columnSpan: 4 - text: qsTr("Show/Hide Grid") - checked: true - onClicked: { - GridConfig.OnShow(checked) - } - } - - // Right spacer - Item { - Layout.columnSpan: 1 - Layout.rowSpan: 15 - Layout.fillWidth: true - } - - Text { - Layout.columnSpan: 4 - text: "Cell Count" - color: "dimgrey" - font.bold: true - } - - Text { - Layout.columnSpan: 2 - id: vercelltext - color: "dimgrey" - text: "Vertical" - } - - IgnSpinBox { - Layout.columnSpan: 2 - id: verticalCellCount - maximumValue: 1000 - minimumValue: 0 - value: 0 - onEditingFinished: GridConfig.UpdateVCellCount(verticalCellCount.value) - } - - Text { - Layout.columnSpan: 2 - id: honcelltext - color: "dimgrey" - text: "Horizontal" - } - - IgnSpinBox { - Layout.columnSpan: 2 - id: horizontalCellCount - maximumValue: 1000 - minimumValue: 1 - value: 20 - onEditingFinished: GridConfig.UpdateHCellCount(horizontalCellCount.value) - } - - Text { - Layout.columnSpan: 4 - id: celllengthtext - text: "Cell Length" - color: "dimgrey" - font.bold: true - } - - Text { - Layout.columnSpan: 2 - id: length - color: "dimgrey" - text: "Length (m)" - } - IgnSpinBox { - Layout.columnSpan: 2 - id: cellLength - maximumValue: 1000.00 - minimumValue: 0.01 - value: 1.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.UpdateCellLength(cellLength.value) - } - - Text { - Layout.columnSpan: 2 - id: cartesian - color: "dimgrey" - font.bold: true - text: "Position (m)" - } - - Text { - Layout.columnSpan: 2 - id: principal - text: "Rotation (rad)" - color: "dimgrey" - font.bold: true - } - - Text { - text: "X" - color: "dimgrey" - } - - IgnSpinBox { - id: x - value: 0.00 - maximumValue: 1000.00 - minimumValue: -1000.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - text: "Roll" - color: "dimgrey" - } - - IgnSpinBox { - id: roll - maximumValue: 6.28 - minimumValue: 0.00 - value: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - text: "Y" - color: "dimgrey" - } - - IgnSpinBox { - id: y - value: 0.00 - maximumValue: 1000.00 - minimumValue: -1000.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - text: "Pitch" - color: "dimgrey" - } - - IgnSpinBox { - id: pitch - maximumValue: 6.28 - minimumValue: 0.00 - value: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - text: "Z" - color: "dimgrey" - } - - IgnSpinBox { - id: z - value: 0.00 - maximumValue: 1000.00 - minimumValue: -1000.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - text: "Yaw" - color: "dimgrey" - } - - IgnSpinBox { - id: yaw - maximumValue: 6.28 - minimumValue: 0.00 - value: 0.00 - decimals: 2 - stepSize: 0.01 - onEditingFinished: GridConfig.SetPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - - Text { - Layout.columnSpan: 4 - text: "Color" - color: "dimgrey" - font.bold: true - } - - Text { - text: "R" - color: "dimgrey" - } - - IgnSpinBox { - id: r - maximumValue: 1.00 - minimumValue: 0.00 - value: 0.7 - stepSize: 0.01 - decimals: 2 - onEditingFinished: GridConfig.SetColor(r.value, g.value, b.value, a.value) - } - - Text { - text: "G" - color: "dimgrey" - } - - IgnSpinBox { - id: g - maximumValue: 1.00 - minimumValue: 0.00 - value: 0.7 - stepSize: 0.01 - decimals: 2 - onEditingFinished: GridConfig.SetColor(r.value, g.value, b.value, a.value) - } - - Text { - text: "B" - color: "dimgrey" - } - - IgnSpinBox { - id: b - maximumValue: 1.00 - minimumValue: 0.00 - value: 0.7 - stepSize: 0.01 - decimals: 2 - onEditingFinished: GridConfig.SetColor(r.value, g.value, b.value, a.value) - } - - Text { - text: "A" - color: "dimgrey" - } - - IgnSpinBox { - id: a - maximumValue: 1.00 - minimumValue: 0.00 - value: 1.0 - stepSize: 0.01 - decimals: 2 - onEditingFinished: GridConfig.SetColor(r.value, g.value, b.value, a.value) - } - - Button { - Layout.alignment: Qt.AlignHCenter - Layout.columnSpan: 4 - id: color - text: qsTr("Custom Color") - onClicked: colorDialog.open() - - ColorDialog { - id: colorDialog - title: "Choose a grid color" - visible: false - onAccepted: { - r.value = colorDialog.color.r - g.value = colorDialog.color.g - b.value = colorDialog.color.b - a.value = colorDialog.color.a - GridConfig.SetColor(colorDialog.color.r, colorDialog.color.g, colorDialog.color.b, colorDialog.color.a) - colorDialog.close() - } - onRejected: { - colorDialog.close() - } - } - } - - // Bottom spacer - Item { - Layout.columnSpan: 4 - Layout.fillHeight: true - } -} - diff --git a/src/gui/plugins/grid_config/GridConfig.qrc b/src/gui/plugins/grid_config/GridConfig.qrc deleted file mode 100644 index 107fb5f0780..00000000000 --- a/src/gui/plugins/grid_config/GridConfig.qrc +++ /dev/null @@ -1,5 +0,0 @@ - - - GridConfig.qml - - diff --git a/src/gui/plugins/joint_position_controller/JointPositionController.cc b/src/gui/plugins/joint_position_controller/JointPositionController.cc index 98caba34e4b..c800f14afff 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController.cc @@ -240,12 +240,7 @@ void JointPositionController::Update(const UpdateInfo &, // Add joint to list else { - // TODO(louise) Blocking here is not the best idea - QMetaObject::invokeMethod(&this->dataPtr->jointsModel, - "AddJoint", - Qt::BlockingQueuedConnection, - Q_RETURN_ARG(QStandardItem *, item), - Q_ARG(Entity, jointEntity)); + item = this->dataPtr->jointsModel.AddJoint(jointEntity); newItem = true; } diff --git a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc index 7d1badccc9b..19f2a0da85b 100644 --- a/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc +++ b/src/gui/plugins/joint_position_controller/JointPositionController_TEST.cc @@ -29,7 +29,6 @@ #include #include #include -#include #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" @@ -38,11 +37,11 @@ #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" #include "ignition/gazebo/components/ParentEntity.hh" -#include "ignition/gazebo/gui/GuiRunner.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/test_config.hh" #include "../../../../test/helpers/EnvTestFixture.hh" +#include "../../GuiRunner.hh" #include "JointPositionController.hh" int g_argc = 1; @@ -64,10 +63,7 @@ TEST_F(JointPositionControllerGui, IGN_UTILS_TEST_ENABLED_ONLY_ON_LINUX(Load)) app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle gazebo::gui plugins - // TODO(anyone) Remove deprecation guard once GuiRunner becomes private - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION auto runner = new gazebo::GuiRunner("test"); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->setParent(gui::App()); // Add plugin @@ -151,10 +147,7 @@ TEST_F(JointPositionControllerGui, app->AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); // Create GUI runner to handle gazebo::gui plugins - // TODO(anyone) Remove deprecation guard once GuiRunner becomes private - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION auto runner = new gazebo::GuiRunner("test"); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION runner->setParent(gui::App()); // Load plugin diff --git a/src/gui/plugins/lights/Lights.cc b/src/gui/plugins/lights/Lights.cc index 0d5ee229494..81ff6554efd 100644 --- a/src/gui/plugins/lights/Lights.cc +++ b/src/gui/plugins/lights/Lights.cc @@ -33,7 +33,7 @@ #include #include "ignition/gazebo/EntityComponentManager.hh" -#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/Primitives.hh" namespace ignition::gazebo { @@ -66,85 +66,15 @@ void Lights::LoadConfig(const tinyxml2::XMLElement *) void Lights::OnNewLightClicked(const QString &_sdfString) { std::string modelSdfString = _sdfString.toStdString(); - std::transform(modelSdfString.begin(), modelSdfString.end(), - modelSdfString.begin(), ::tolower); + modelSdfString = getPrimitive(modelSdfString); - if (modelSdfString == "point") + if (!modelSdfString.empty()) { - modelSdfString = std::string("" - "" - "" - "0 0 2 0 0 0" - "false" - "0.5 0.5 0.5 1" - "0.5 0.5 0.5 1" - "" - "4" - "0.2" - "0.5" - "0.01" - "" - "" - ""); + ignition::gui::events::SpawnFromDescription event(modelSdfString); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); } - else if (modelSdfString == "directional") - { - modelSdfString = std::string("" - "" - "" - "0 0 2 0 0 0" - "true" - "0.8 0.8 0.8 1" - "0.2 0.2 0.2 1" - "" - "1000" - "0.9" - "0.01" - "0.001" - "" - "0 0 -1" - "" - ""); - } - else if (modelSdfString == "spot") - { - modelSdfString = std::string("" - "" - "" - "0 0 2 0 0 0" - "true" - "0.5 0.5 0.5 1" - "0.5 0.5 0.5 1" - "" - "4" - "0.2" - "0.5" - "0.01" - "" - "0 0 -1" - "" - "0.1" - "0.5" - "0.8" - "" - "" - ""); - } - else - { - ignwarn << "Invalid model string " << modelSdfString << "\n"; - ignwarn << "The valid options are:\n"; - ignwarn << " - point\n"; - ignwarn << " - directional\n"; - ignwarn << " - spot\n"; - return; - } - - ignition::gui::events::SpawnFromDescription event(modelSdfString); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &event); } // Register this plugin diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index e76cbd8cf11..6d722b83a17 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -15,6 +15,7 @@ * */ +#include "../../GuiRunner.hh" #include "EntityContextMenu.hh" #include @@ -25,7 +26,6 @@ #include #include -#include #include #include #include @@ -47,9 +47,30 @@ namespace ignition::gazebo /// \brief Remove service name public: std::string removeService; + /// \brief View as transparent service name + public: std::string viewTransparentService; + + /// \brief View center of mass service name + public: std::string viewCOMService; + + /// \brief View inertia service name + public: std::string viewInertiaService; + + /// \brief View joints service name + public: std::string viewJointsService; + + /// \brief View wireframes service name + public: std::string viewWireframesService; + /// \brief View collisions service name public: std::string viewCollisionsService; + /// \brief Copy service name + public: std::string copyService; + + /// \brief Paste service name + public: std::string pasteService; + /// \brief Name of world. public: std::string worldName; }; @@ -79,8 +100,29 @@ EntityContextMenu::EntityContextMenu() // For remove service requests this->dataPtr->removeService = "/world/default/remove"; + // For view transparent service requests + this->dataPtr->viewTransparentService = "/gui/view/transparent"; + + // For view center of mass service requests + this->dataPtr->viewCOMService = "/gui/view/com"; + + // For view inertia service requests + this->dataPtr->viewInertiaService = "/gui/view/inertia"; + + // For view joints service requests + this->dataPtr->viewJointsService = "/gui/view/joints"; + + // For view wireframes service requests + this->dataPtr->viewWireframesService = "/gui/view/wireframes"; + // For view collisions service requests this->dataPtr->viewCollisionsService = "/gui/view/collisions"; + + // For copy service requests + this->dataPtr->copyService = "/gui/copy"; + + // For paste service requests + this->dataPtr->pasteService = "/gui/paste"; } ///////////////////////////////////////////////// @@ -152,12 +194,53 @@ 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_transparent") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewTransparentService, req, cb); + } + else if (request == "view_com") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewCOMService, req, cb); + } + else if (request == "view_inertia") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewInertiaService, req, cb); + } + else if (request == "view_joints") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewJointsService, req, cb); + } + else if (request == "view_wireframes") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->viewWireframesService, 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 if (request == "copy") + { + ignition::msgs::StringMsg req; + req.set_data(_data.toStdString()); + this->dataPtr->node.Request(this->dataPtr->copyService, req, cb); + } + else if (request == "paste") + { + ignition::msgs::Empty req; + this->dataPtr->node.Request(this->dataPtr->pasteService, 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 bb2f4e02bef..02940eaba5c 100644 --- a/src/gui/plugins/modules/EntityContextMenu.qml +++ b/src/gui/plugins/modules/EntityContextMenu.qml @@ -37,6 +37,18 @@ Item { text: "Remove" onTriggered: context.OnRemove(context.entity, context.type) } + MenuItem { + id: copyMenu + text: "Copy" + onTriggered: context.OnRequest("copy", context.entity) + } + MenuItem { + id: pasteMenu + text: "Paste" + // no data needs to be passed along to the paste request, so an empty + // string is sent + onTriggered: context.OnRequest("paste", "") + } // // cascading submenu only works in Qt 5.10+ on focal // Menu { // id: viewSubmenu @@ -63,6 +75,14 @@ Item { id: secondMenu x: menu.x + menu.width y: menu.y + viewSubmenu.y + MenuItem { + id: viewCOMMenu + text: "Center of Mass" + onTriggered: { + menu.close() + context.OnRequest("view_com", context.entity) + } + } MenuItem { id: viewCollisionsMenu text: "Collisions" @@ -71,6 +91,38 @@ Item { context.OnRequest("view_collisions", context.entity) } } + MenuItem { + id: viewInertiaMenu + text: "Inertia" + onTriggered: { + menu.close() + context.OnRequest("view_inertia", context.entity) + } + } + MenuItem { + id: viewJointsMenu + text: "Joints" + onTriggered: { + menu.close() + context.OnRequest("view_joints", context.entity) + } + } + MenuItem { + id: viewTransparentMenu + text: "Transparent" + onTriggered: { + menu.close() + context.OnRequest("view_transparent", context.entity) + } + } + MenuItem { + id: viewWireframesMenu + text: "Wireframe" + onTriggered: { + menu.close() + context.OnRequest("view_wireframes", context.entity) + } + } } function open(_entity, _type, _x, _y) { @@ -81,6 +133,11 @@ Item { moveToMenu.enabled = false followMenu.enabled = false removeMenu.enabled = false + viewTransparentMenu.enabled = false; + viewCOMMenu.enabled = false; + viewInertiaMenu.enabled = false; + viewJointsMenu.enabled = false; + viewWireframesMenu.enabled = false; viewCollisionsMenu.enabled = false; // enable / disable menu items @@ -99,9 +156,18 @@ Item { if (context.type == "model" || context.type == "link") { + viewTransparentMenu.enabled = true; + viewCOMMenu.enabled = true; + viewInertiaMenu.enabled = true; + viewWireframesMenu.enabled = true; viewCollisionsMenu.enabled = true; } + if (context.type == "model") + { + viewJointsMenu.enabled = true; + } + menu.open() } diff --git a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc index 9f4606091b9..2935caaf1cb 100644 --- a/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc +++ b/src/gui/plugins/playback_scrubber/PlaybackScrubber.cc @@ -38,7 +38,6 @@ #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/components/LogPlaybackStatistics.hh" -#include "ignition/gazebo/gui/GuiEvents.hh" namespace ignition::gazebo { diff --git a/src/gui/plugins/plot_3d/Plot3D_TEST.cc b/src/gui/plugins/plot_3d/Plot3D_TEST.cc index ea4d3fbd81d..916a0609b3b 100644 --- a/src/gui/plugins/plot_3d/Plot3D_TEST.cc +++ b/src/gui/plugins/plot_3d/Plot3D_TEST.cc @@ -42,9 +42,7 @@ #include "ignition/gazebo/test_config.hh" #include "../../../../test/helpers/EnvTestFixture.hh" -// Use this when forward-porting to v6 -// #include "../../GuiRunner.hh" -#include "ignition/gazebo/gui/GuiRunner.hh" +#include "../../GuiRunner.hh" #include "Plot3D.hh" diff --git a/src/gui/plugins/plotting/Plotting.cc b/src/gui/plugins/plotting/Plotting.cc index 94d91357baf..a6f5228ae59 100644 --- a/src/gui/plugins/plotting/Plotting.cc +++ b/src/gui/plugins/plotting/Plotting.cc @@ -33,6 +33,7 @@ #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/Static.hh" #include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" @@ -72,6 +73,7 @@ namespace ignition::gazebo }; } +using namespace ignition; using namespace ignition::gazebo; using namespace ignition::gui; @@ -130,6 +132,13 @@ PlotComponent::PlotComponent(const std::string &_type, this->dataPtr->data["stepSize"] = std::make_shared(); this->dataPtr->data["realTimeFactor"] = std::make_shared(); } + else if (_type == "SphericalCoordinates") + { + this->dataPtr->data["latitude"] = std::make_shared(); + this->dataPtr->data["longitude"] = std::make_shared(); + this->dataPtr->data["elevation"] = std::make_shared(); + this->dataPtr->data["heading"] = std::make_shared(); + } else ignwarn << "Invalid Plot Component Type:" << _type << std::endl; } @@ -306,6 +315,20 @@ 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 math::SphericalCoordinates &_sc) +{ + std::lock_guard lock(this->dataPtr->componentsMutex); + this->dataPtr->components[_Id]->SetAttributeValue("latitude", + _sc.LatitudeReference().Degree()); + this->dataPtr->components[_Id]->SetAttributeValue("longitude", + _sc.LongitudeReference().Degree()); + this->dataPtr->components[_Id]->SetAttributeValue("elevation", + _sc.ElevationReference()); + this->dataPtr->components[_Id]->SetAttributeValue("heading", + _sc.HeadingOffset().Degree()); +} + ////////////////////////////////////////////////// void Plotting::SetData(std::string _Id, const sdf::Physics &_physics) { @@ -448,6 +471,12 @@ void Plotting::Update(const ignition::gazebo::UpdateInfo &_info, if (comp) this->SetData(component.first, comp->Data()); } + else if (typeId == components::SphericalCoordinates::typeId) + { + auto comp = _ecm.Component(entity); + if (comp) + this->SetData(component.first, comp->Data()); + } else if (typeId == components::TrajectoryPose::typeId) { auto comp = _ecm.Component(entity); diff --git a/src/gui/plugins/plotting/Plotting.hh b/src/gui/plugins/plotting/Plotting.hh index 534d9a5008c..9e85c62264c 100644 --- a/src/gui/plugins/plotting/Plotting.hh +++ b/src/gui/plugins/plotting/Plotting.hh @@ -23,6 +23,7 @@ #include #include #include +#include #include #include "sdf/Physics.hh" @@ -110,31 +111,37 @@ class Plotting : public ignition::gazebo::GuiSystem public: void Update(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; - /// \brief Set the Component data of giving id to the giving vector + /// \brief Set the Component data of given id to the given vector /// \param [in] _Id Component Key of the components map /// \param [in] _vector Vector Data to be set to the component public: void SetData(std::string _Id, const ignition::math::Vector3d &_vector); - /// \brief Set the Component data of giving id to the giving light + /// \brief Set the Component data of given id to the given 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 + /// \brief Set the Component data of given id to the given 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 + /// \brief Set the Component data of given id to the given spherical + /// coordinates. + /// \param [in] _Id Component Key of the components map + /// \param [in] _sc Data to be set to the component + public: void SetData(std::string _Id, const math::SphericalCoordinates &_sc); + + /// \brief Set the Component data of given id to the given /// 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 + /// \brief Set the Component data of given id to the given number /// \param [in] _Id Component Key of the components map /// \param [in] _value double Data to be set to the component /// valid for types (double, float, int, bool) diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 806dac60a21..9320e88be44 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -38,9 +38,8 @@ #include #include #include -#include -#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/EntityComponentManager.hh" namespace ignition::gazebo { @@ -631,14 +630,6 @@ void ResourceSpawner::OnResourceSpawn(const QString &_sdfPath) ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &event); - - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - ignition::gazebo::gui::events::SpawnPreviewPath oldEvent( - _sdfPath.toStdString()); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &oldEvent); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // Register this plugin diff --git a/src/gui/plugins/scene3d/Scene3D.cc b/src/gui/plugins/scene3d/Scene3D.cc index 423f5b11551..e33d8be79f5 100644 --- a/src/gui/plugins/scene3d/Scene3D.cc +++ b/src/gui/plugins/scene3d/Scene3D.cc @@ -49,6 +49,7 @@ #include #include +#include #include #include #include @@ -58,8 +59,6 @@ #include -#include - #include #include #include @@ -87,6 +86,7 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Helper to store selection requests to be handled in the render /// thread by `IgnRenderer::HandleEntitySelection`. + // SelectEntities struct SelectionHelper { /// \brief Entity to be selected @@ -102,29 +102,36 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Private data class for IgnRenderer class IgnRendererPrivate { - /// \brief Flag to indicate if mouse event is dirty - public: bool mouseDirty = false; + // -------------------------------------------------------------- + // InteractiveViewControl - /// \brief Flag to indicate if hover event is dirty - public: bool hoverDirty = false; + /// \brief Orbit view controller + public: rendering::OrbitViewController orbitViewControl; - /// \brief Mouse event - public: common::MouseEvent mouseEvent; + /// \brief Ortho view controller + public: rendering::OrthoViewController orthoViewControl; - /// \brief Key event - public: common::KeyEvent keyEvent; + /// \brief Camera view controller + public: rendering::ViewController *viewControl{nullptr}; - /// \brief Mouse move distance since last event. - public: math::Vector2d drag; + /// \brief View controller + public: std::string viewController{"orbit"}; - /// \brief Mutex to protect mouse events - public: std::mutex mutex; + /// \brief View control focus target + public: math::Vector3d target = math::Vector3d( + math::INF_D, math::INF_D, math::INF_D); - /// \brief User camera - public: rendering::CameraPtr camera; + // -------------------------------------------------------------- + // TransformControl + + /// \brief The xyz values by which to snap the object. + public: math::Vector3d xyzSnap = math::Vector3d::One; - /// \brief Camera orbit controller - public: rendering::OrbitViewController viewControl; + /// \brief The rpy values by which to snap the object. + public: math::Vector3d rpySnap = {45, 45, 45}; + + /// \brief The scale values by which to snap the object. + public: math::Vector3d scaleSnap = math::Vector3d::One; /// \brief Transform controller for models public: rendering::TransformController transformControl; @@ -137,6 +144,28 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: rendering::TransformMode transformMode = rendering::TransformMode::TM_NONE; + /// \brief Name of service for setting entity pose + public: std::string poseCmdService; + + /// \brief Flag to indicate whether the x key is currently being pressed + public: bool xPressed = false; + + /// \brief Flag to indicate whether the y key is currently being pressed + public: bool yPressed = false; + + /// \brief Flag to indicate whether the z key is currently being pressed + public: bool zPressed = false; + + /// \brief The starting world pose of a clicked visual. + public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; + + /// \brief Flag to keep track of world pose setting used + /// for button translating. + public: bool isStartWorldPosSet = false; + + // -------------------------------------------------------------- + // VideoRecorder + /// \brief True to record a video from the user camera public: bool recordVideo = false; @@ -166,19 +195,21 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Video recording statistics publisher public: transport::Node::Publisher recorderStatsPub; + /// \brief Image from user camera + public: rendering::Image cameraImage; + + /// \brief Video encoder + public: common::VideoEncoder videoEncoder; + + // -------------------------------------------------------------- + // CameraTracking + /// \brief Target to move the user camera to public: std::string moveToTarget; /// \brief Helper object to move user camera public: ignition::rendering::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; - /// \brief Target to follow public: std::string followTarget; @@ -201,8 +232,42 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// false to follow in target's local frame public: bool followWorldFrame = false; - /// \brief Flag for indicating whether we are in view angle mode or not - public: bool viewAngle = false; + /// \brief The pose set from the move to pose service. + public: std::optional moveToPoseValue; + + /// \brief Last move to animation time + public: std::chrono::time_point prevMoveToTime; + + // -------------------------------------------------------------- + // VisualizationCapabilities + + /// \brief Target to view as transparent + public: std::string viewTransparentTarget; + + /// \brief Target to view center of mass + public: std::string viewCOMTarget; + + /// \brief Target to view inertia + public: std::string viewInertiaTarget; + + /// \brief Target to view joints + public: std::string viewJointsTarget; + + /// \brief Target to view wireframes + public: std::string viewWireframesTarget; + + /// \brief Target to view collisions + public: std::string viewCollisionsTarget; + + // -------------------------------------------------------------- + // SelectEntities + + /// \brief Helper object to select entities. Only the latest selection + /// request is kept. + public: SelectionHelper selectionHelper; + + // -------------------------------------------------------------- + // Spawn /// \brief Flag for indicating whether we are spawning or not. public: bool isSpawning = false; @@ -211,10 +276,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// resource with the shapes plugin or not public: bool isPlacing = false; - /// \brief Atomic bool indicating whether the dropdown menu - /// is currently enabled or disabled. - public: std::atomic_bool dropdownMenuEnabled = true; - /// \brief The SDF string of the resource to be used with plugins that spawn /// entities. public: std::string spawnSdfString; @@ -226,9 +287,6 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { public: ignition::math::Pose3d spawnPreviewPose = ignition::math::Pose3d::Zero; - /// \brief The currently hovered mouse position in screen coordinates - public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; - /// \brief The visual generated from the spawnSdfString / spawnSdfPath public: rendering::NodePtr spawnPreview = nullptr; @@ -236,78 +294,71 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// for easy deletion of visuals later public: std::vector previewIds; + /// \brief Name of service for creating entity + public: std::string createCmdService; + + // -------------------------------------------------------------- + // ViewAngle + + /// \brief Flag for indicating whether we are in view angle mode or not + public: bool viewAngle = false; + /// \brief The pose set during a view angle button press that holds /// the pose the camera should assume relative to the entit(y/ies). /// The vector (0, 0, 0) indicates to return the camera back to the home /// pose originally loaded from the sdf. public: math::Vector3d viewAngleDirection = math::Vector3d::Zero; - /// \brief The pose set from the move to pose service. - public: std::optional moveToPoseValue; + // -------------------------------------------------------------- + // Common to various plugins - /// \brief Last move to animation time - public: std::chrono::time_point prevMoveToTime; + /// \brief Flag to indicate if mouse event is dirty + public: bool mouseDirty = false; - /// \brief Image from user camera - public: rendering::Image cameraImage; + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty = false; - /// \brief Video encoder - public: common::VideoEncoder videoEncoder; + /// \brief Mouse event + public: common::MouseEvent mouseEvent; + + /// \brief Key event + public: common::KeyEvent keyEvent; + + /// \brief Mouse move distance since last event. + public: math::Vector2d drag; + + /// \brief Mutex to protect mouse events + public: std::mutex mutex; + + /// \brief User camera + public: rendering::CameraPtr camera; + + /// \brief Atomic bool indicating whether the dropdown menu + /// is currently enabled or disabled. + public: std::atomic_bool dropdownMenuEnabled = true; + + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; /// \brief Ray query for mouse clicks public: rendering::RayQueryPtr rayQuery; - /// \brief View control focus target - public: math::Vector3d target = math::Vector3d( - math::INF_D, math::INF_D, math::INF_D); - /// \brief Rendering utility public: RenderUtil renderUtil; /// \brief Transport node for making transform control requests public: transport::Node node; - /// \brief Name of service for setting entity pose - public: std::string poseCmdService; - - /// \brief Name of service for creating entity - public: std::string createCmdService; - - /// \brief The starting world pose of a clicked visual. - public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; - - /// \brief Flag to keep track of world pose setting used - /// for button translating. - public: bool isStartWorldPosSet = false; - /// \brief Where the mouse left off - used to continue translating /// smoothly when switching axes through keybinding and clicking /// Updated on an x, y, or z, press or release and a mouse press public: math::Vector2i mousePressPos = math::Vector2i::Zero; - /// \brief Flag to indicate whether the x key is currently being pressed - public: bool xPressed = false; - - /// \brief Flag to indicate whether the y key is currently being pressed - public: bool yPressed = false; - - /// \brief Flag to indicate whether the z key is currently being pressed - public: bool zPressed = false; - /// \brief Flag to indicate whether the escape key has been released. public: bool escapeReleased = false; /// \brief ID of thread where render calls can be made. public: std::thread::id renderThreadId; - - /// \brief The xyz values by which to snap the object. - public: math::Vector3d xyzSnap = math::Vector3d::One; - - /// \brief The rpy values by which to snap the object. - public: math::Vector3d rpySnap = {45, 45, 45}; - - /// \brief The scale values by which to snap the object. - public: math::Vector3d scaleSnap = math::Vector3d::One; }; /// \brief Qt and Ogre rendering is happening in different threads @@ -415,21 +466,30 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Private data class for Scene3D class Scene3DPrivate { - /// \brief Transport node - public: transport::Node node; - - /// \brief Name of the world - public: std::string worldName; - - /// \brief Rendering utility - public: RenderUtil *renderUtil = nullptr; + // -------------------------------------------------------------- + // TransformControl /// \brief Transform mode service public: std::string transformModeService; + // -------------------------------------------------------------- + // VideoRecorder + /// \brief Record video service public: std::string recordVideoService; + /// \brief lockstep ECM updates with rendering + public: bool recordVideoLockstep = false; + + /// \brief True to indicate video recording in progress + public: bool recording = false; + + /// \brief mutex to protect the recording variable + public: std::mutex recordMutex; + + // -------------------------------------------------------------- + // CameraTracking + /// \brief Move to service public: std::string moveToService; @@ -439,37 +499,67 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \brief Follow offset service public: std::string followOffsetService; - /// \brief View angle service - public: std::string viewAngleService; - /// \brief Move to pose service public: std::string moveToPoseService; - /// \brief Shapes service - public: std::string shapesService; - /// \brief Camera pose topic public: std::string cameraPoseTopic; /// \brief Camera pose publisher public: transport::Node::Publisher cameraPosePub; - /// \brief lockstep ECM updates with rendering - public: bool recordVideoLockstep = false; + // -------------------------------------------------------------- + // VisualizationCapabilities - /// \brief True to indicate video recording in progress - public: bool recording = false; + /// \brief View transparent service + public: std::string viewTransparentService; - /// \brief mutex to protect the recording variable - public: std::mutex recordMutex; + /// \brief View center of mass service + public: std::string viewCOMService; - /// \brief mutex to protect the render condition variable - /// Used when recording in lockstep mode. - public: std::mutex renderMutex; + /// \brief View inertia service + public: std::string viewInertiaService; + + /// \brief View joints service + public: std::string viewJointsService; + + /// \brief View wireframes service + public: std::string viewWireframesService; /// \brief View collisions service public: std::string viewCollisionsService; + // -------------------------------------------------------------- + // InteractiveViewControl + + /// \brief Camera view control service + public: std::string cameraViewControlService; + + // -------------------------------------------------------------- + // GzSceneManager + + /// \brief Rendering utility + public: RenderUtil *renderUtil = nullptr; + + // -------------------------------------------------------------- + // ViewAngle + + /// \brief View angle service + public: std::string viewAngleService; + + // -------------------------------------------------------------- + // Common to various plugins + + /// \brief Transport node + public: transport::Node node; + + /// \brief Name of the world + public: std::string worldName; + + /// \brief mutex to protect the render condition variable + /// Used when recording in lockstep mode. + public: std::mutex renderMutex; + /// \brief Text for popup error message public: QString errorPopupText; }; @@ -600,9 +690,11 @@ void IgnRenderer::Render(RenderSync *_renderSync) // setting the size should cause the render texture to be rebuilt { IGN_PROFILE("IgnRenderer::Render Pre-render camera"); - this->dataPtr->camera->PreRender(); - this->dataPtr->camera->Render(); + this->dataPtr->camera->Update(); } + // mark mouse dirty to force update view projection in HandleMouseEvent + this->dataPtr->mouseDirty = true; + this->textureDirty = false; // TODO(anyone) See SwapFromThread comments @@ -925,6 +1017,136 @@ void IgnRenderer::Render(RenderSync *_renderSync) } } + // View as transparent + { + IGN_PROFILE("IgnRenderer::Render ViewTransparent"); + if (!this->dataPtr->viewTransparentTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewTransparentTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewTransparent(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewTransparentTarget + << "] to view as transparent" << std::endl; + } + + this->dataPtr->viewTransparentTarget.clear(); + } + } + + // View center of mass + { + IGN_PROFILE("IgnRenderer::Render ViewCOM"); + if (!this->dataPtr->viewCOMTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewCOMTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewCOM(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewCOMTarget + << "] to view center of mass" << std::endl; + } + + this->dataPtr->viewCOMTarget.clear(); + } + } + + // View inertia + { + IGN_PROFILE("IgnRenderer::Render ViewInertia"); + if (!this->dataPtr->viewInertiaTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewInertiaTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewInertia(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewInertiaTarget + << "] to view inertia" << std::endl; + } + + this->dataPtr->viewInertiaTarget.clear(); + } + } + + // View joints + { + IGN_PROFILE("IgnRenderer::Render ViewJoints"); + if (!this->dataPtr->viewJointsTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewJointsTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewJoints(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewJointsTarget + << "] to view joints" << std::endl; + } + + this->dataPtr->viewJointsTarget.clear(); + } + } + + // View wireframes + { + IGN_PROFILE("IgnRenderer::Render ViewWireframes"); + if (!this->dataPtr->viewWireframesTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->dataPtr->viewWireframesTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->dataPtr->renderUtil.ViewWireframes(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->dataPtr->viewWireframesTarget + << "] to view wireframes" << std::endl; + } + + this->dataPtr->viewWireframesTarget.clear(); + } + } + // View collisions { IGN_PROFILE("IgnRenderer::Render ViewCollisions"); @@ -934,7 +1156,7 @@ void IgnRenderer::Render(RenderSync *_renderSync) scene->NodeByName(this->dataPtr->viewCollisionsTarget); auto targetVis = std::dynamic_pointer_cast(targetNode); - if (targetVis) + if (targetVis && targetVis->HasUserData("gazebo-entity")) { Entity targetEntity = std::get(targetVis->UserData("gazebo-entity")); @@ -957,13 +1179,6 @@ void IgnRenderer::Render(RenderSync *_renderSync) ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &event); - - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - ignition::gazebo::gui::events::Render oldEvent; - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &oldEvent); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // only has an effect in video recording lockstep mode @@ -1059,12 +1274,10 @@ bool IgnRenderer::GeneratePreview(const sdf::Root &_sdf) } this->dataPtr->spawnPreview = this->dataPtr->renderUtil.SceneManager().CreateLight( - lightId, light, + lightId, light, light.Name(), this->dataPtr->renderUtil.SceneManager().WorldId()); this->dataPtr->renderUtil.SceneManager().CreateLightVisual( - lightVisualId, light, lightId); - - + lightVisualId, light, light.Name(), lightId); this->dataPtr->previewIds.push_back(lightId); this->dataPtr->previewIds.push_back(lightVisualId); @@ -1799,17 +2012,35 @@ void IgnRenderer::HandleMouseViewControl() << std::endl; } - this->dataPtr->viewControl.SetCamera(this->dataPtr->camera); + if (!this->dataPtr->followTarget.empty()) + this->dataPtr->camera->WorldPosition(); + + if (this->dataPtr->viewController == "ortho") + { + this->dataPtr->viewControl = &this->dataPtr->orthoViewControl; + } + else if (this->dataPtr->viewController == "orbit") + { + this->dataPtr->viewControl = &this->dataPtr->orbitViewControl; + } + else + { + ignerr << "Unknown view controller: " << this->dataPtr->viewController + << ". Defaulting to orbit view controller" << std::endl; + this->dataPtr->viewController = "orbit"; + this->dataPtr->viewControl = &this->dataPtr->orbitViewControl; + } + this->dataPtr->viewControl->SetCamera(this->dataPtr->camera); if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::SCROLL) { this->dataPtr->target = this->ScreenToScene(this->dataPtr->mouseEvent.Pos()); - this->dataPtr->viewControl.SetTarget(this->dataPtr->target); + this->dataPtr->viewControl->SetTarget(this->dataPtr->target); double distance = this->dataPtr->camera->WorldPosition().Distance( this->dataPtr->target); double amount = -this->dataPtr->drag.Y() * distance / 5.0; - this->dataPtr->viewControl.Zoom(amount); + this->dataPtr->viewControl->Zoom(amount); } else { @@ -1824,7 +2055,7 @@ void IgnRenderer::HandleMouseViewControl() { this->dataPtr->target = this->ScreenToScene( this->dataPtr->mouseEvent.PressPos()); - this->dataPtr->viewControl.SetTarget(this->dataPtr->target); + this->dataPtr->viewControl->SetTarget(this->dataPtr->target); } // unset the target on release (by setting to inf) else if (this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) @@ -1836,14 +2067,14 @@ void IgnRenderer::HandleMouseViewControl() if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::LEFT) { if (Qt::ShiftModifier == QGuiApplication::queryKeyboardModifiers()) - this->dataPtr->viewControl.Orbit(this->dataPtr->drag); + this->dataPtr->viewControl->Orbit(this->dataPtr->drag); else - this->dataPtr->viewControl.Pan(this->dataPtr->drag); + this->dataPtr->viewControl->Pan(this->dataPtr->drag); } // Orbit with middle button else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::MIDDLE) { - this->dataPtr->viewControl.Orbit(this->dataPtr->drag); + this->dataPtr->viewControl->Orbit(this->dataPtr->drag); } else if (this->dataPtr->mouseEvent.Buttons() & common::MouseEvent::RIGHT) { @@ -1855,7 +2086,7 @@ void IgnRenderer::HandleMouseViewControl() double amount = ((-this->dataPtr->drag.Y() / static_cast(this->dataPtr->camera->ImageHeight())) * distance * tan(vfov/2.0) * 6.0); - this->dataPtr->viewControl.Zoom(amount); + this->dataPtr->viewControl->Zoom(amount); } } this->dataPtr->drag = 0; @@ -1872,6 +2103,9 @@ void IgnRenderer::Initialize() if (this->initialized) return; + this->dataPtr->renderUtil.SetWinID(std::to_string( + ignition::gui::App()->findChild()-> + QuickWindow()->winId())); this->dataPtr->renderUtil.SetUseCurrentGLContext(true); this->dataPtr->renderUtil.Init(); @@ -1881,8 +2115,11 @@ void IgnRenderer::Initialize() auto root = scene->RootVisual(); + scene->SetCameraPassCountPerGpuFlush(6u); + // Camera this->dataPtr->camera = scene->CreateCamera(); + this->dataPtr->camera->SetUserData("user-camera", true); root->AddChild(this->dataPtr->camera); this->dataPtr->camera->SetLocalPose(this->cameraPose); this->dataPtr->camera->SetImageWidth(this->textureSize.width()); @@ -2127,6 +2364,41 @@ void IgnRenderer::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->moveToPoseValue = _pose; } +///////////////////////////////////////////////// +void IgnRenderer::SetViewTransparentTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewTransparentTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewCOMTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewCOMTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewInertiaTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewInertiaTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewJointsTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewJointsTarget = _target; +} + +///////////////////////////////////////////////// +void IgnRenderer::SetViewWireframesTarget(const std::string &_target) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewWireframesTarget = _target; +} + ///////////////////////////////////////////////// void IgnRenderer::SetViewCollisionsTarget(const std::string &_target) { @@ -2134,6 +2406,17 @@ void IgnRenderer::SetViewCollisionsTarget(const std::string &_target) this->dataPtr->viewCollisionsTarget = _target; } +///////////////////////////////////////////////// +void IgnRenderer::SetViewController(const std::string &_controller) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->viewController = _controller; + + // mark mouse dirty to trigger HandleMouseEvent call and + // set up a new view controller + this->dataPtr->mouseDirty = true; +} + ///////////////////////////////////////////////// void IgnRenderer::SetFollowPGain(double _gain) { @@ -2627,10 +2910,12 @@ RenderUtil *RenderWindowItem::RenderUtil() const Scene3D::Scene3D() : GuiSystem(), dataPtr(new Scene3DPrivate) { + ignwarn << "The GzScene3D plugin is deprecated on v6 and will be removed on " + << "v7. Use MinimalScene together with other plugins as needed." + << std::endl; qmlRegisterType("RenderWindow", 1, 0, "RenderWindow"); } - ///////////////////////////////////////////////// Scene3D::~Scene3D() = default; @@ -2901,6 +3186,41 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "Camera pose topic advertised on [" << this->dataPtr->cameraPoseTopic << "]" << std::endl; + // view as transparent service + this->dataPtr->viewTransparentService = "/gui/view/transparent"; + this->dataPtr->node.Advertise(this->dataPtr->viewTransparentService, + &Scene3D::OnViewTransparent, this); + ignmsg << "View as transparent service on [" + << this->dataPtr->viewTransparentService << "]" << std::endl; + + // view center of mass service + this->dataPtr->viewCOMService = "/gui/view/com"; + this->dataPtr->node.Advertise(this->dataPtr->viewCOMService, + &Scene3D::OnViewCOM, this); + ignmsg << "View center of mass service on [" + << this->dataPtr->viewCOMService << "]" << std::endl; + + // view inertia service + this->dataPtr->viewInertiaService = "/gui/view/inertia"; + this->dataPtr->node.Advertise(this->dataPtr->viewInertiaService, + &Scene3D::OnViewInertia, this); + ignmsg << "View inertia service on [" + << this->dataPtr->viewInertiaService << "]" << std::endl; + + // view joints service + this->dataPtr->viewJointsService = "/gui/view/joints"; + this->dataPtr->node.Advertise(this->dataPtr->viewJointsService, + &Scene3D::OnViewJoints, this); + ignmsg << "View joints service on [" + << this->dataPtr->viewJointsService << "]" << std::endl; + + // view wireframes service + this->dataPtr->viewWireframesService = "/gui/view/wireframes"; + this->dataPtr->node.Advertise(this->dataPtr->viewWireframesService, + &Scene3D::OnViewWireframes, this); + ignmsg << "View wireframes service on [" + << this->dataPtr->viewWireframesService << "]" << std::endl; + // view collisions service this->dataPtr->viewCollisionsService = "/gui/view/collisions"; this->dataPtr->node.Advertise(this->dataPtr->viewCollisionsService, @@ -2908,6 +3228,13 @@ void Scene3D::LoadConfig(const tinyxml2::XMLElement *_pluginElem) ignmsg << "View collisions service on [" << this->dataPtr->viewCollisionsService << "]" << std::endl; + // camera view control mode + this->dataPtr->cameraViewControlService = "/gui/camera/view_control"; + this->dataPtr->node.Advertise(this->dataPtr->cameraViewControlService, + &Scene3D::OnViewControl, this); + ignmsg << "Camera view controller topic advertised on [" + << this->dataPtr->cameraViewControlService << "]" << std::endl; + ignition::gui::App()->findChild< ignition::gui::MainWindow *>()->QuickWindow()->installEventFilter(this); ignition::gui::App()->findChild< @@ -3076,6 +3403,66 @@ bool Scene3D::OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res) return true; } +///////////////////////////////////////////////// +bool Scene3D::OnViewTransparent(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewTransparentTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewCOM(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewCOMTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewInertia(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewInertiaTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewJoints(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewJointsTarget(_msg.data()); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool Scene3D::OnViewWireframes(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewWireframesTarget(_msg.data()); + + _res.set_data(true); + return true; +} + ///////////////////////////////////////////////// bool Scene3D::OnViewCollisions(const msgs::StringMsg &_msg, msgs::Boolean &_res) @@ -3088,6 +3475,19 @@ bool Scene3D::OnViewCollisions(const msgs::StringMsg &_msg, return true; } +///////////////////////////////////////////////// +bool Scene3D::OnViewControl(const msgs::StringMsg &_msg, + msgs::Boolean &_res) +{ + auto renderWindow = this->PluginItem()->findChild(); + + renderWindow->SetViewController(_msg.data()); + + _res.set_data(true); + return true; +} + + ///////////////////////////////////////////////// void Scene3D::OnHovered(int _mouseX, int _mouseY) { @@ -3408,12 +3808,48 @@ void RenderWindowItem::SetMoveToPose(const math::Pose3d &_pose) this->dataPtr->renderThread->ignRenderer.SetMoveToPose(_pose); } +///////////////////////////////////////////////// +void RenderWindowItem::SetViewTransparentTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewTransparentTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewCOMTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewCOMTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewInertiaTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewInertiaTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewJointsTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewJointsTarget(_target); +} + +///////////////////////////////////////////////// +void RenderWindowItem::SetViewWireframesTarget(const std::string &_target) +{ + this->dataPtr->renderThread->ignRenderer.SetViewWireframesTarget(_target); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetViewCollisionsTarget(const std::string &_target) { this->dataPtr->renderThread->ignRenderer.SetViewCollisionsTarget(_target); } +///////////////////////////////////////////////// +void RenderWindowItem::SetViewController(const std::string &_controller) +{ + this->dataPtr->renderThread->ignRenderer.SetViewController(_controller); +} + ///////////////////////////////////////////////// void RenderWindowItem::SetFollowPGain(double _gain) { diff --git a/src/gui/plugins/scene3d/Scene3D.hh b/src/gui/plugins/scene3d/Scene3D.hh index c2462fe4cf0..28c15227bee 100644 --- a/src/gui/plugins/scene3d/Scene3D.hh +++ b/src/gui/plugins/scene3d/Scene3D.hh @@ -169,6 +169,43 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { private: bool OnMoveToPose(const msgs::GUICamera &_msg, msgs::Boolean &_res); + /// \brief Callback for view as transparent request + /// \param[in] _msg Request message to set the target to view as + /// transparent + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewTransparent(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view center of mass request + /// \param[in] _msg Request message to set the target to view center of + /// mass + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewCOM(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view inertia request + /// \param[in] _msg Request message to set the target to view inertia + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewInertia(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view joints request + /// \param[in] _msg Request message to set the target to view joints + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewJoints(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Callback for view wireframes request + /// \param[in] _msg Request message to set the target to view wireframes + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewWireframes(const msgs::StringMsg &_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 @@ -192,6 +229,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// the connection to work on the QML side signals: void popupError(); + /// \brief Callback for camera view controller request + /// \param[in] _msg Request message to set the camera view controller + /// \param[in] _res Response data + /// \return True if the request is received + private: bool OnViewControl(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; @@ -287,10 +331,34 @@ 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 the specified target as transparent + /// \param[in] _target Target to view as transparent + public: void SetViewTransparentTarget(const std::string &_target); + + /// \brief View center of mass of the specified target + /// \param[in] _target Target to view center of mass + public: void SetViewCOMTarget(const std::string &_target); + + /// \brief View inertia of the specified target + /// \param[in] _target Target to view inertia + public: void SetViewInertiaTarget(const std::string &_target); + + /// \brief View joints of the specified target + /// \param[in] _target Target to view joints + public: void SetViewJointsTarget(const std::string &_target); + + /// \brief View wireframes of the specified target + /// \param[in] _target Target to view wireframes + public: void SetViewWireframesTarget(const std::string &_target); + /// \brief View collisions of the specified target /// \param[in] _target Target to view collisions public: void SetViewCollisionsTarget(const std::string &_target); + /// \brief Set camera view controller + /// \param[in] _viewController. Values are "orbit", and "ortho" + public: void SetViewController(const std::string &_viewController); + /// \brief Set the p gain for the camera follow movement /// \param[in] _gain Camera follow p gain. public: void SetFollowPGain(double _gain); @@ -638,10 +706,34 @@ 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 the specified target as transparent + /// \param[in] _target Target to view as transparent + public: void SetViewTransparentTarget(const std::string &_target); + + /// \brief View center of mass of the specified target + /// \param[in] _target Target to view center of mass + public: void SetViewCOMTarget(const std::string &_target); + + /// \brief View inertia of the specified target + /// \param[in] _target Target to view inertia + public: void SetViewInertiaTarget(const std::string &_target); + + /// \brief View joints of the specified target + /// \param[in] _target Target to view joints + public: void SetViewJointsTarget(const std::string &_target); + + /// \brief View wireframes of the specified target + /// \param[in] _target Target to view wireframes + public: void SetViewWireframesTarget(const std::string &_target); + /// \brief View collisions of the specified target /// \param[in] _target Target to view collisions public: void SetViewCollisionsTarget(const std::string &_target); + /// \brief Set camera view controller + /// \param[in] _viewController. Values are "orbit", and "ortho" + public: void SetViewController(const std::string &_viewController); + /// \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/plugins/scene_manager/CMakeLists.txt b/src/gui/plugins/scene_manager/CMakeLists.txt new file mode 100644 index 00000000000..6e5e884b940 --- /dev/null +++ b/src/gui/plugins/scene_manager/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_gui_plugin(GzSceneManager + SOURCES GzSceneManager.cc + QT_HEADERS GzSceneManager.hh + PRIVATE_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering + ignition-utils${IGN_UTILS_VER}::ignition-utils${IGN_UTILS_VER} +) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc new file mode 100644 index 00000000000..88710b0e9b4 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -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. + * +*/ + +#include "GzSceneManager.hh" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +namespace ignition +{ +namespace gazebo +{ +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + /// \brief Private data class for GzSceneManager + class GzSceneManagerPrivate + { + /// \brief Update the 3D scene based on the latest state of the ECM. + public: void OnRender(); + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene; + + /// \brief Rendering utility + public: RenderUtil renderUtil; + }; +} +} +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +GzSceneManager::GzSceneManager() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +GzSceneManager::~GzSceneManager() = default; + +///////////////////////////////////////////////// +void GzSceneManager::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Scene Manager"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +////////////////////////////////////////////////// +void GzSceneManager::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("GzSceneManager::Update"); + + this->dataPtr->renderUtil.UpdateECM(_info, _ecm); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + + // Emit entities created / removed event for gui::Plugins which don't have + // direct access to the ECM. + std::set created; + _ecm.EachNew( + [&](const Entity &_entity, const components::Name *)->bool + { + created.insert(_entity); + return true; + }); + std::set removed; + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Name *)->bool + { + removed.insert(_entity); + return true; + }); + + ignition::gazebo::gui::events::NewRemovedEntities removedEvent( + created, removed); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &removedEvent); +} + +///////////////////////////////////////////////// +bool GzSceneManager::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void GzSceneManagerPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + this->renderUtil.SetScene(this->scene); + } + + this->renderUtil.Update(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::GzSceneManager, + ignition::gui::Plugin) diff --git a/src/gui/plugins/scene_manager/GzSceneManager.hh b/src/gui/plugins/scene_manager/GzSceneManager.hh new file mode 100644 index 00000000000..ab0d5f1be0d --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.hh @@ -0,0 +1,66 @@ +/* + * 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_GZSCENEMANAGER_HH_ +#define IGNITION_GAZEBO_GUI_GZSCENEMANAGER_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + class GzSceneManagerPrivate; + + /// \brief Updates a 3D scene based on information coming from the ECM. + /// This plugin doesn't instantiate a new 3D scene. Instead, it relies on + /// another plugin being loaded alongside it that will create and paint the + /// scene to the window, such as `ignition::gui::plugins::Scene3D`. + class GzSceneManager : public GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: GzSceneManager(); + + /// \brief Destructor + public: ~GzSceneManager() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} + +#endif diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qml b/src/gui/plugins/scene_manager/GzSceneManager.qml new file mode 100644 index 00000000000..873da30014a --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qml @@ -0,0 +1,28 @@ +/* + * 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.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/scene_manager/GzSceneManager.qrc b/src/gui/plugins/scene_manager/GzSceneManager.qrc new file mode 100644 index 00000000000..b28c13c3b46 --- /dev/null +++ b/src/gui/plugins/scene_manager/GzSceneManager.qrc @@ -0,0 +1,5 @@ + + + GzSceneManager.qml + + diff --git a/src/gui/plugins/select_entities/CMakeLists.txt b/src/gui/plugins/select_entities/CMakeLists.txt new file mode 100644 index 00000000000..e8d86d7d986 --- /dev/null +++ b/src/gui/plugins/select_entities/CMakeLists.txt @@ -0,0 +1,11 @@ +gz_add_gui_plugin(SelectEntities + SOURCES + SelectEntities.cc + QT_HEADERS + SelectEntities.hh + TEST_SOURCES + # CameraControllerManager_TEST.cc + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc new file mode 100644 index 00000000000..d3513888f78 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -0,0 +1,602 @@ +/* + * 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 "ignition/rendering/Camera.hh" + +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" + +#include "SelectEntities.hh" + +namespace ignition +{ +namespace gazebo +{ +namespace gui +{ +/// \brief Helper to store selection requests to be handled in the render +/// thread by `IgnRenderer::HandleEntitySelection`. +struct SelectionHelper +{ + /// \brief Entity to be selected + Entity selectEntity{kNullEntity}; + + /// \brief Deselect all entities + bool deselectAll{false}; + + /// \brief True to send an event and notify all widgets + bool sendEvent{false}; +}; +} +} +} + +/// \brief Private data class for SelectEntities +class ignition::gazebo::gui::SelectEntitiesPrivate +{ + /// \brief Initialize the plugin, attaching to a camera. + public: void Initialize(); + + /// \brief Handle entity selection in the render thread. + public: void HandleEntitySelection(); + + /// \brief Select new entity + /// \param[in] _visual Visual that was clicked + /// \param[in] _sendEvent True to send an event and notify other widgets + public: void UpdateSelectedEntity(const rendering::VisualPtr &_visual, + bool _sendEvent); + + /// \brief Highlight a selected rendering node + /// \param[in] _visual Node to be highlighted + public: void HighlightNode(const rendering::VisualPtr &_visual); + + /// \brief Remove highlight from a rendering node that's no longer selected + /// \param[in] _visual Node to be lowlighted + public: void LowlightNode(const rendering::VisualPtr &_visual); + + /// \brief Select the entity for the given visual + /// \param[in] _visual Visual to select + public: void SetSelectedEntity(const rendering::VisualPtr &_visual); + + /// \brief Deselect all selected entities. + public: void DeselectAllEntities(); + + /// \brief Get the ancestor of a given node which is a direct child of the + /// world. + /// \param[in] _node Node to get ancestor of + /// \return Top level node. + public: rendering::NodePtr TopLevelNode( + const rendering::NodePtr &_node); + + /// \brief Helper object to select entities. Only the latest selection + /// request is kept. + public: SelectionHelper selectionHelper; + + /// \brief Currently selected entities, organized by order of selection. + /// These are ign-gazebo IDs + public: std::vector selectedEntities; + + /// \brief Currently selected entities, organized by order of selection. + /// These are ign-rendering IDs + public: std::vector selectedEntitiesID; + + /// \brief New entities received from other plugins. + /// These are ign-rendering IDs + public: std::vector selectedEntitiesIDNew; + + //// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene = nullptr; + + /// \brief A map of entity ids and wire boxes + public: std::unordered_map wireBoxes; + + /// \brief MouseEvent + public: ignition::common::MouseEvent mouseEvent; + + /// \brief is the mouse modify ? + public: bool mouseDirty = false; + + /// \brief selected entities from other plugins (for example: entityTree) + public: bool receivedSelectedEntities = false; + + /// \brief User camera + public: rendering::CameraPtr camera = nullptr; + + /// \brief is transform control active ? + public: bool transformControlActive = false; + + /// \brief Is an entity being spawned + public: bool isSpawning{false}; +}; + +using namespace ignition; +using namespace gazebo; +using namespace gazebo::gui; + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::HandleEntitySelection() +{ + if (this->receivedSelectedEntities) + { + if (!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier)) + { + this->DeselectAllEntities(); + } + + for (unsigned int i = 0; i < this->selectedEntitiesIDNew.size(); i++) + { + auto visualToHighLight = this->scene->VisualById( + this->selectedEntitiesIDNew[i]); + + if (nullptr == visualToHighLight) + { + ignerr << "Failed to get visual with ID [" + << this->selectedEntitiesIDNew[i] << "]" << std::endl; + continue; + } + + this->selectedEntitiesID.push_back(this->selectedEntitiesIDNew[i]); + + Entity entityId = kNullEntity; + try + { + entityId = std::get(visualToHighLight->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &_e) + { + // It's ok to get here + } + + this->selectedEntities.push_back(entityId); + + this->HighlightNode(visualToHighLight); + + ignition::gazebo::gui::events::EntitiesSelected selectEvent( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &selectEvent); + } + this->receivedSelectedEntities = false; + this->selectionHelper = SelectionHelper(); + this->selectedEntitiesIDNew.clear(); + } + + if (!mouseDirty) + return; + + this->mouseDirty = false; + + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + this->DeselectAllEntities(); + return; + } + + Entity entityId = kNullEntity; + try + { + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) + { + // It's ok to get here + } + + this->selectionHelper.selectEntity = entityId; + + if (this->selectionHelper.deselectAll) + { + this->DeselectAllEntities(); + + this->selectionHelper = SelectionHelper(); + } + else if (this->selectionHelper.selectEntity != kNullEntity) + { + this->UpdateSelectedEntity(visual, this->selectionHelper.sendEvent); + + this->selectionHelper = SelectionHelper(); + } +} + +//////////////////////////////////////////////// +void SelectEntitiesPrivate::LowlightNode(const rendering::VisualPtr &_visual) +{ + Entity entityId = kNullEntity; + if (_visual) + { + try + { + entityId = std::get(_visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + } + if (this->wireBoxes.find(entityId) != this->wireBoxes.end()) + { + ignition::rendering::WireBoxPtr wireBox = this->wireBoxes[entityId]; + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(false); + } +} + +//////////////////////////////////////////////// +void SelectEntitiesPrivate::HighlightNode(const rendering::VisualPtr &_visual) +{ + if (nullptr == _visual) + { + ignerr << "Failed to highlight null visual." << std::endl; + return; + } + + Entity entityId = kNullEntity; + try + { + entityId = std::get(_visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + + // If the entity is not found in the existing map, create a wire box + auto wireBoxIt = this->wireBoxes.find(entityId); + if (wireBoxIt == this->wireBoxes.end()) + { + auto white = this->scene->Material("highlight_material"); + if (!white) + { + white = this->scene->CreateMaterial("highlight_material"); + white->SetAmbient(1.0, 1.0, 1.0); + white->SetDiffuse(1.0, 1.0, 1.0); + white->SetSpecular(1.0, 1.0, 1.0); + white->SetEmissive(1.0, 1.0, 1.0); + } + + ignition::rendering::WireBoxPtr wireBox = this->scene->CreateWireBox(); + ignition::math::AxisAlignedBox aabb = _visual->LocalBoundingBox(); + wireBox->SetBox(aabb); + + // Create visual and add wire box + ignition::rendering::VisualPtr wireBoxVis = this->scene->CreateVisual(); + wireBoxVis->SetInheritScale(false); + wireBoxVis->AddGeometry(wireBox); + wireBoxVis->SetMaterial(white, false); + wireBoxVis->SetUserData("gui-only", static_cast(true)); + _visual->AddChild(wireBoxVis); + + // Add wire box to map for setting visibility + this->wireBoxes.insert( + std::pair(entityId, wireBox)); + } + else + { + ignition::rendering::WireBoxPtr wireBox = wireBoxIt->second; + ignition::math::AxisAlignedBox aabb = _visual->LocalBoundingBox(); + wireBox->SetBox(aabb); + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(true); + } +} + +///////////////////////////////////////////////// +rendering::NodePtr SelectEntitiesPrivate::TopLevelNode( + const rendering::NodePtr &_node) +{ + if (!this->scene) + return rendering::NodePtr(); + + rendering::NodePtr rootNode = this->scene->RootVisual(); + + rendering::NodePtr nodeTmp = _node; + while (nodeTmp && nodeTmp->Parent() != rootNode) + { + nodeTmp = + std::dynamic_pointer_cast(nodeTmp->Parent()); + } + + return nodeTmp; +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::SetSelectedEntity( + const rendering::VisualPtr &_visual) +{ + if (nullptr == _visual) + { + ignerr << "Failed to select null visual" << std::endl; + return; + } + + Entity entityId = kNullEntity; + + auto topLevelNode = this->TopLevelNode(_visual); + auto topLevelVisual = std::dynamic_pointer_cast( + topLevelNode); + + if (topLevelVisual) + { + try + { + entityId = std::get(topLevelVisual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + } + + if (entityId == kNullEntity) + return; + + this->selectedEntities.push_back(entityId); + this->selectedEntitiesID.push_back(topLevelVisual->Id()); + this->HighlightNode(topLevelVisual); + ignition::gazebo::gui::events::EntitiesSelected entitiesSelected( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &entitiesSelected); +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::DeselectAllEntities() +{ + if (nullptr == this->scene) + return; + + for (const auto &entity : this->selectedEntitiesID) + { + auto node = this->scene->VisualById(entity); + auto vis = std::dynamic_pointer_cast(node); + this->LowlightNode(vis); + } + this->selectedEntities.clear(); + this->selectedEntitiesID.clear(); + + ignition::gazebo::gui::events::DeselectAllEntities deselectEvent(true); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &deselectEvent); +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::UpdateSelectedEntity( + const rendering::VisualPtr &_visual, bool _sendEvent) +{ + bool deselectedAll{false}; + + // Deselect all if control is not being held + if ((!(QGuiApplication::keyboardModifiers() & Qt::ControlModifier) && + !this->selectedEntitiesID.empty()) || this->transformControlActive) + { + // Notify other widgets regardless of _sendEvent, because this is a new + // decision from this widget + this->DeselectAllEntities(); + deselectedAll = true; + } + + // Select new entity + this->SetSelectedEntity(_visual); + + // Notify other widgets of the currently selected entities + if (_sendEvent || deselectedAll) + { + ignition::gazebo::gui::events::EntitiesSelected selectEvent( + this->selectedEntities); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &selectEvent); + } +} + +///////////////////////////////////////////////// +void SelectEntitiesPrivate::Initialize() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + return; + + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "SelectEntities plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->camera) + { + ignerr << "TransformControl camera is not available" << std::endl; + return; + } + } +} + +///////////////////////////////////////////////// +SelectEntities::SelectEntities() + : dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +SelectEntities::~SelectEntities() = default; + +///////////////////////////////////////////////// +void SelectEntities::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Select entities"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + + if (this->dataPtr->mouseEvent.Button() == common::MouseEvent::LEFT && + this->dataPtr->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + if (this->dataPtr->isSpawning) + { + this->dataPtr->isSpawning = false; + } + else + { + this->dataPtr->mouseDirty = true; + } + } + } + else if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->Initialize(); + this->dataPtr->HandleEntitySelection(); + } + else if (_event->type() == + ignition::gazebo::gui::events::TransformControlModeActive::kType) + { + auto transformControlMode = + reinterpret_cast( + _event); + this->dataPtr->transformControlActive = + transformControlMode->TransformControlActive(); + } + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) + { + auto selectedEvent = + reinterpret_cast(_event); + if (selectedEvent && !selectedEvent->Data().empty() && + selectedEvent->FromUser()) + { + for (const auto &entity : selectedEvent->Data()) + { + for (unsigned int i = 0; i < this->dataPtr->scene->VisualCount(); i++) + { + auto visual = this->dataPtr->scene->VisualByIndex(i); + + Entity entityId = kNullEntity; + try + { + entityId = std::get(visual->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &) + { + // It's ok to get here + } + + if (entityId == entity) + { + this->dataPtr->selectedEntitiesIDNew.push_back(visual->Id()); + this->dataPtr->receivedSelectedEntities = true; + break; + } + } + } + } + } + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntitiesID.clear(); + this->dataPtr->selectedEntities.clear(); + } + else if (_event->type() == + ignition::gui::events::SpawnFromDescription::kType || + _event->type() == ignition::gui::events::SpawnFromPath::kType) + { + this->dataPtr->isSpawning = true; + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) + { + ignition::gui::events::KeyReleaseOnScene *_e = + static_cast(_event); + if (_e->Key().Key() == Qt::Key_Escape) + { + this->dataPtr->mouseDirty = true; + this->dataPtr->selectionHelper.deselectAll = true; + this->dataPtr->isSpawning = false; + } + } + else if (_event->type() == + ignition::gazebo::gui::events::NewRemovedEntities::kType) + { + if (!this->dataPtr->wireBoxes.empty()) + { + auto event = + reinterpret_cast(_event); + for (auto &entity : event->RemovedEntities()) + { + auto wireBoxIt = this->dataPtr->wireBoxes.find(entity); + if (wireBoxIt != this->dataPtr->wireBoxes.end()) + { + this->dataPtr->scene->DestroyVisual(wireBoxIt->second->Parent()); + this->dataPtr->wireBoxes.erase(wireBoxIt); + } + } + + } + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::gui::SelectEntities, + ignition::gui::Plugin) diff --git a/src/gui/plugins/select_entities/SelectEntities.hh b/src/gui/plugins/select_entities/SelectEntities.hh new file mode 100644 index 00000000000..cdbd3a172c6 --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.hh @@ -0,0 +1,59 @@ +/* + * 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_SELECTENTITIES_HH_ +#define IGNITION_GAZEBO_GUI_SELECTENTITIES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +namespace gui +{ + class SelectEntitiesPrivate; + + /// \brief This plugin is in charge of selecting and deselecting the entities + /// from the Scene3D and emit the corresponding events. + class SelectEntities : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Constructor + public: SelectEntities(); + + /// \brief Destructor + public: virtual ~SelectEntities(); + + // Documentation inherited + public: virtual void LoadConfig(const tinyxml2::XMLElement *_pluginElem) + override; + + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} +} +#endif diff --git a/src/gui/plugins/select_entities/SelectEntities.qml b/src/gui/plugins/select_entities/SelectEntities.qml new file mode 100644 index 00000000000..873da30014a --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.qml @@ -0,0 +1,28 @@ +/* + * 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.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/select_entities/SelectEntities.qrc b/src/gui/plugins/select_entities/SelectEntities.qrc new file mode 100644 index 00000000000..d303772321f --- /dev/null +++ b/src/gui/plugins/select_entities/SelectEntities.qrc @@ -0,0 +1,5 @@ + + + SelectEntities.qml + + diff --git a/src/gui/plugins/shapes/Shapes.cc b/src/gui/plugins/shapes/Shapes.cc index 4bb4291be89..acd25ee6d1b 100644 --- a/src/gui/plugins/shapes/Shapes.cc +++ b/src/gui/plugins/shapes/Shapes.cc @@ -31,9 +31,8 @@ #include #include #include -#include -#include "ignition/gazebo/gui/GuiEvents.hh" +#include namespace ignition::gazebo { @@ -75,216 +74,15 @@ void Shapes::LoadConfig(const tinyxml2::XMLElement *) void Shapes::OnMode(const QString &_mode) { std::string modelSdfString = _mode.toStdString(); - std::transform(modelSdfString.begin(), modelSdfString.end(), - modelSdfString.begin(), ::tolower); + modelSdfString = getPrimitive(modelSdfString); - if (modelSdfString == "box") + if (!modelSdfString.empty()) { - modelSdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.16666" - "0" - "0" - "0.16666" - "0" - "0.16666" - "" - "1.0" - "" - "" - "" - "" - "1 1 1" - "" - "" - "" - "" - "" - "" - "1 1 1" - "" - "" - "" - "" - "" - ""); + ignition::gui::events::SpawnFromDescription event(modelSdfString); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &event); } - else if (modelSdfString == "sphere") - { - modelSdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.1" - "0" - "0" - "0.1" - "0" - "0.1" - "" - "1.0" - "" - "" - "" - "" - "0.5" - "" - "" - "" - "" - "" - "" - "0.5" - "" - "" - "" - "" - "" - ""); - } - else if (modelSdfString == "cylinder") - { - modelSdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.1458" - "0" - "0" - "0.1458" - "0" - "0.125" - "" - "1.0" - "" - "" - "" - "" - "0.5" - "1.0" - "" - "" - "" - "" - "" - "" - "0.5" - "1.0" - "" - "" - "" - "" - "" - ""); - } - else if (modelSdfString == "capsule") - { - modelSdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.074154" - "0" - "0" - "0.074154" - "0" - "0.018769" - "" - "1.0" - "" - "" - "" - "" - "0.2" - "0.6" - "" - "" - "" - "" - "" - "" - "0.2" - "0.6" - "" - "" - "" - "" - "" - ""); - } - else if (modelSdfString == "ellipsoid") - { - modelSdfString = std::string("" - "" - "" - "0 0 0.5 0 0 0" - "" - "" - "" - "0.068" - "0" - "0" - "0.058" - "0" - "0.026" - "" - "1.0" - "" - "" - "" - "" - "0.2 0.3 0.5" - "" - "" - "" - "" - "" - "" - "0.2 0.3 0.5" - "" - "" - "" - "" - "" - ""); - } - else - { - ignwarn << "Invalid model string " << modelSdfString << "\n"; - ignwarn << "The valid options are:\n"; - ignwarn << " - box\n"; - ignwarn << " - sphere\n"; - ignwarn << " - capsule\n"; - ignwarn << " - cylinder\n"; - ignwarn << " - ellipsoid\n"; - return; - } - - ignition::gui::events::SpawnFromDescription event(modelSdfString); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &event); - - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - ignition::gazebo::gui::events::SpawnPreviewModel oldEvent(modelSdfString); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &oldEvent); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } // Register this plugin diff --git a/src/gui/plugins/spawn/CMakeLists.txt b/src/gui/plugins/spawn/CMakeLists.txt new file mode 100644 index 00000000000..dada40b6b38 --- /dev/null +++ b/src/gui/plugins/spawn/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(Spawn + SOURCES + Spawn.cc + QT_HEADERS + Spawn.hh + PUBLIC_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/spawn/Spawn.cc b/src/gui/plugins/spawn/Spawn.cc new file mode 100644 index 00000000000..d49ecd982d9 --- /dev/null +++ b/src/gui/plugins/spawn/Spawn.cc @@ -0,0 +1,679 @@ +/* + * 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 "Spawn.hh" + +#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 +#include + +#include + +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/rendering/SceneManager.hh" + +namespace ignition::gazebo +{ + class SpawnPrivate + { + /// \brief Perform operations in the render thread. + public: void OnRender(); + + /// \brief Delete the visuals generated while an entity is being spawned. + public: void TerminateSpawnPreview(); + + /// \brief Generate a preview of a resource. + /// \param[in] _sdf The SDF to be previewed. + /// \return True on success, false if failure + public: bool GeneratePreview(const sdf::Root &_sdf); + + /// \brief Generate a preview of a resource. + /// \param[in] _sdf The name of the resource to be previewed. + /// \return True on success, false if failure + public: bool GeneratePreview(const std::string &_name); + + /// \brief Handle placement requests + public: void HandlePlacement(); + + /// \brief Retrieve the point on a plane at z = 0 in the 3D scene hit by a + /// ray cast from the given 2D screen coordinates. + /// \param[in] _screenPos 2D coordinates on the screen, in pixels. + /// \param[in] _camera User camera + /// \param[in] _rayQuery Ray query for mouse clicks + /// \param[in] _offset Offset along the plane normal + /// \return 3D coordinates of a point in the 3D scene. + math::Vector3d ScreenToPlane( + const math::Vector2i &_screenPos, + const rendering::CameraPtr &_camera, + const rendering::RayQueryPtr &_rayQuery, + const float offset = 0.0); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Flag for indicating whether the preview needs to be generated. + public: bool generatePreview = false; + + /// \brief Flag for indicating whether the user is currently placing a + /// resource or not + public: bool isPlacing = false; + + /// \brief The SDF string of the resource to be used with plugins that spawn + /// entities. + public: std::string spawnSdfString; + + /// \brief Path of an SDF file, to be used with plugins that spawn entities. + public: std::string spawnSdfPath; + + /// \brief The name of a resource to clone + public: std::string spawnCloneName; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief A record of the ids currently used by the entity spawner + /// for easy deletion of visuals later + public: std::vector previewIds; + + /// \brief Pointer to the preview that the user is placing. + public: rendering::NodePtr spawnPreview{nullptr}; + + /// \brief Scene manager + public: SceneManager sceneManager; + + /// \brief The pose of the spawn preview. + public: math::Pose3d spawnPreviewPose = + math::Pose3d::Zero; + + /// \brief Mouse event + public: common::MouseEvent mouseEvent; + + /// \brief Flag to indicate if mouse event is dirty + public: bool mouseDirty = false; + + /// \brief Flag to indicate if hover event is dirty + public: bool hoverDirty = false; + + /// \brief Flag to indicate whether the escape key has been released. + public: bool escapeReleased = false; + + /// \brief The currently hovered mouse position in screen coordinates + public: math::Vector2i mouseHoverPos = math::Vector2i::Zero; + + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Name of service for creating entity + public: std::string createCmdService; + + /// \brief Name of the world + public: std::string worldName; + + /// \brief Text for popup error message + public: QString errorPopupText; + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +Spawn::Spawn() + : ignition::gui::Plugin(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +Spawn::~Spawn() = default; + +///////////////////////////////////////////////// +void Spawn::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "Spawn"; + + // World name from window, to construct default topics and services + auto worldNames = gui::worldNames(); + if (!worldNames.empty()) + this->dataPtr->worldName = worldNames[0].toStdString(); + + ignition::gui::App()->findChild + ()->installEventFilter(this); +} + + +// TODO(ahcorde): Replace this when this function is on ign-rendering6 +///////////////////////////////////////////////// +math::Vector3d SpawnPrivate::ScreenToPlane( + const math::Vector2i &_screenPos, + const rendering::CameraPtr &_camera, + const rendering::RayQueryPtr &_rayQuery, + const float offset) +{ + // Normalize point on the image + double width = _camera->ImageWidth(); + double height = _camera->ImageHeight(); + + double nx = 2.0 * _screenPos.X() / width - 1.0; + double ny = 1.0 - 2.0 * _screenPos.Y() / height; + + // Make a ray query + _rayQuery->SetFromCamera( + _camera, math::Vector2d(nx, ny)); + + math::Planed plane(math::Vector3d(0, 0, 1), offset); + + math::Vector3d origin = _rayQuery->Origin(); + math::Vector3d direction = _rayQuery->Direction(); + double distance = plane.Distance(origin, direction); + return origin + direction * distance; +} + +///////////////////////////////////////////////// +void SpawnPrivate::HandlePlacement() +{ + if (!this->isPlacing) + return; + + if (this->spawnPreview && this->hoverDirty) + { + math::Vector3d pos = this->ScreenToPlane( + this->mouseHoverPos, this->camera, this->rayQuery); + pos.Z(this->spawnPreview->WorldPosition().Z()); + this->spawnPreview->SetWorldPosition(pos); + this->hoverDirty = false; + } + if (this->mouseEvent.Button() == common::MouseEvent::LEFT && + this->mouseEvent.Type() == common::MouseEvent::RELEASE && + !this->mouseEvent.Dragging() && this->mouseDirty) + { + // Delete the generated visuals + this->TerminateSpawnPreview(); + + auto pose = this->spawnPreviewPose; + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error creating entity" << std::endl; + }; + math::Vector3d pos = this->ScreenToPlane( + this->mouseEvent.Pos(), this->camera, this->rayQuery); + pos.Z(pose.Pos().Z()); + msgs::EntityFactory req; + if (!this->spawnSdfString.empty()) + { + req.set_sdf(this->spawnSdfString); + } + else if (!this->spawnSdfPath.empty()) + { + req.set_sdf_filename(this->spawnSdfPath); + } + else if (!this->spawnCloneName.empty()) + { + req.set_clone_name(this->spawnCloneName); + } + else + { + ignwarn << "Failed to find SDF string or file path" << std::endl; + } + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), math::Pose3d(pos, pose.Rot())); + + if (this->createCmdService.empty()) + { + this->createCmdService = "/world/" + this->worldName + + "/create"; + } + this->createCmdService = transport::TopicUtils::AsValidTopic( + this->createCmdService); + if (this->createCmdService.empty()) + { + ignerr << "Failed to create valid create command service for world [" + << this->worldName <<"]" << std::endl; + return; + } + + this->node.Request(this->createCmdService, req, cb); + this->isPlacing = false; + this->mouseDirty = false; + this->spawnSdfString.clear(); + this->spawnSdfPath.clear(); + this->spawnCloneName.clear(); + } +} + +///////////////////////////////////////////////// +void SpawnPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + { + return; + } + this->sceneManager.SetScene(this->scene); + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + + // Ray Query + this->rayQuery = this->camera->Scene()->CreateRayQuery(); + + igndbg << "Spawn plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + } + + // Spawn + IGN_PROFILE("IgnRenderer::Render Spawn"); + if (this->generatePreview) + { + bool cloningResource = false; + + // Generate spawn preview + rendering::VisualPtr rootVis = this->scene->RootVisual(); + sdf::Root root; + if (!this->spawnSdfString.empty()) + { + root.LoadSdfString(this->spawnSdfString); + } + else if (!this->spawnSdfPath.empty()) + { + root.Load(this->spawnSdfPath); + } + else if (!this->spawnCloneName.empty()) + { + this->isPlacing = this->GeneratePreview(this->spawnCloneName); + cloningResource = true; + } + else + { + ignwarn << "Failed to spawn: no SDF string, path, or name of resource " + << "to clone" << std::endl; + } + + if (!cloningResource) + this->isPlacing = this->GeneratePreview(root); + + this->generatePreview = false; + } + + // Escape action, clear all selections and terminate any + // spawned previews if escape button is released + { + if (this->escapeReleased) + { + this->TerminateSpawnPreview(); + this->escapeReleased = false; + } + } + + this->HandlePlacement(); +} + +///////////////////////////////////////////////// +void SpawnPrivate::TerminateSpawnPreview() +{ + for (auto _id : this->previewIds) + { + this->sceneManager.RemoveEntity(_id); + } + this->previewIds.clear(); + this->isPlacing = false; +} + +///////////////////////////////////////////////// +bool SpawnPrivate::GeneratePreview(const sdf::Root &_sdf) +{ + // Terminate any pre-existing spawned entities + this->TerminateSpawnPreview(); + + if (nullptr == _sdf.Model() && nullptr == _sdf.Light()) + { + ignwarn << "Only model or light entities can be spawned at the moment." + << std::endl; + return false; + } + + if (_sdf.Model()) + { + // Only preview first model + sdf::Model model = *(_sdf.Model()); + this->spawnPreviewPose = model.RawPose(); + model.SetName(common::Uuid().String()); + Entity modelId = this->sceneManager.UniqueId(); + if (kNullEntity == modelId) + { + this->TerminateSpawnPreview(); + return false; + } + this->spawnPreview = this->sceneManager.CreateModel( + modelId, model, this->sceneManager.WorldId()); + + this->previewIds.push_back(modelId); + for (auto j = 0u; j < model.LinkCount(); j++) + { + sdf::Link link = *(model.LinkByIndex(j)); + link.SetName(common::Uuid().String()); + Entity linkId = this->sceneManager.UniqueId(); + if (!linkId) + { + this->TerminateSpawnPreview(); + return false; + } + this->sceneManager.CreateLink(linkId, link, modelId); + this->previewIds.push_back(linkId); + for (auto k = 0u; k < link.VisualCount(); k++) + { + sdf::Visual visual = *(link.VisualByIndex(k)); + visual.SetName(common::Uuid().String()); + Entity visualId = this->sceneManager.UniqueId(); + if (!visualId) + { + this->TerminateSpawnPreview(); + return false; + } + this->sceneManager.CreateVisual(visualId, visual, linkId); + this->previewIds.push_back(visualId); + } + } + } + else if (_sdf.Light()) + { + // Only preview first light + sdf::Light light = *(_sdf.Light()); + this->spawnPreviewPose = light.RawPose(); + light.SetName(common::Uuid().String()); + Entity lightVisualId = this->sceneManager.UniqueId(); + if (!lightVisualId) + { + this->TerminateSpawnPreview(); + return false; + } + Entity lightId = this->sceneManager.UniqueId(); + if (!lightId) + { + this->TerminateSpawnPreview(); + return false; + } + this->spawnPreview = this->sceneManager.CreateLight( + lightId, light, light.Name(), this->sceneManager.WorldId()); + this->sceneManager.CreateLightVisual( + lightVisualId, light, light.Name(), lightId); + + this->previewIds.push_back(lightId); + this->previewIds.push_back(lightVisualId); + } + return true; +} + +///////////////////////////////////////////////// +bool SpawnPrivate::GeneratePreview(const std::string &_name) +{ + // Terminate any pre-existing spawned entities + this->TerminateSpawnPreview(); + + Entity visualId = this->sceneManager.UniqueId(); + if (!visualId) + { + this->TerminateSpawnPreview(); + return false; + } + + auto visualChildrenPair = this->sceneManager.CopyVisual(visualId, _name, + this->sceneManager.WorldId()); + if (!visualChildrenPair.first) + { + ignerr << "Copying a visual named " << _name << "failed.\n"; + return false; + } + + this->spawnPreview = visualChildrenPair.first; + this->spawnPreviewPose = this->spawnPreview->WorldPose(); + + // save the copied chiled IDs before saving the copied parent visual ID in + // order to ensure that the child visuals get deleted before the parent visual + // (since the SceneManager::RemoveEntity call in this->TerminateSpawnPreview() + // isn't recursive, deleting the parent visual before the child visuals could + // result in dangling child visuals) + const auto &visualChildIds = visualChildrenPair.second; + for (auto reverse_it = visualChildIds.rbegin(); + reverse_it != visualChildIds.rend(); ++reverse_it) + this->previewIds.push_back(*reverse_it); + this->previewIds.push_back(visualId); + + return true; +} + +//////////////////////////////////////////////// +bool Spawn::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + if (this->dataPtr->generatePreview || this->dataPtr->isPlacing) + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::HoverOnScene::kType) + { + ignition::gui::events::HoverOnScene *_e = + static_cast(_event); + this->dataPtr->mouseHoverPos = _e->Mouse().Pos(); + this->dataPtr->hoverDirty = true; + } + else if (_event->type() == + ignition::gui::events::SpawnFromDescription::kType) + { + ignition::gui::events::SpawnFromDescription *_e = + static_cast(_event); + this->dataPtr->spawnSdfString = _e->Description(); + this->dataPtr->generatePreview = true; + } + else if (_event->type() == ignition::gui::events::SpawnFromPath::kType) + { + auto spawnPreviewPathEvent = + reinterpret_cast(_event); + this->dataPtr->spawnSdfPath = spawnPreviewPathEvent->FilePath(); + this->dataPtr->generatePreview = true; + } + else if (_event->type() == ignition::gui::events::SpawnCloneFromName::kType) + { + auto spawnCloneEvent = + reinterpret_cast(_event); + if (spawnCloneEvent) + { + this->dataPtr->spawnCloneName = spawnCloneEvent->Name(); + this->dataPtr->generatePreview = true; + } + } + else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) + { + ignition::gui::events::KeyReleaseOnScene *_e = + static_cast(_event); + if (_e->Key().Key() == Qt::Key_Escape) + { + this->dataPtr->escapeReleased = true; + } + } + else if (_event->type() == ignition::gui::events::DropOnScene::kType) + { + auto dropOnSceneEvent = + reinterpret_cast(_event); + if (dropOnSceneEvent) + { + this->OnDropped(dropOnSceneEvent); + } + } + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void Spawn::OnDropped(const ignition::gui::events::DropOnScene *_event) +{ + if (nullptr == _event || nullptr == this->dataPtr->camera || + nullptr == this->dataPtr->rayQuery) + { + return; + } + + if (_event->DropText().empty()) + { + this->SetErrorPopupText("Dropped empty entity URI."); + return; + } + + std::function cb = + [](const ignition::msgs::Boolean &_res, const bool _result) + { + if (!_result || !_res.data()) + ignerr << "Error creating dropped entity." << std::endl; + }; + + math::Vector3d pos = ignition::rendering::screenToScene( + _event->Mouse(), + this->dataPtr->camera, + this->dataPtr->rayQuery); + + msgs::EntityFactory req; + std::string dropStr = _event->DropText(); + + // Local meshes + if (QUrl(QString::fromStdString(dropStr)).isLocalFile()) + { + // mesh to sdf model + common::rtrim(dropStr); + + if (!common::MeshManager::Instance()->IsValidFilename(dropStr)) + { + QString errTxt = QString::fromStdString("Invalid URI: " + dropStr + + "\nOnly Fuel URLs or mesh file types DAE, OBJ, and STL are supported."); + this->SetErrorPopupText(errTxt); + return; + } + + // Fixes whitespace + dropStr = common::replaceAll(dropStr, "%20", " "); + + std::string filename = common::basename(dropStr); + std::vector splitName = common::split(filename, "."); + + std::string sdf = "" + "" + "" + "" + "" + "" + "" + "" + dropStr + "" + "" + "" + "" + "" + "" + "" + "" + dropStr + "" + "" + "" + "" + "" + "" + ""; + + req.set_sdf(sdf); + } + // Resource from fuel + else + { + req.set_sdf_filename(dropStr); + } + + req.set_allow_renaming(true); + msgs::Set(req.mutable_pose(), + math::Pose3d(pos.X(), pos.Y(), pos.Z(), 1, 0, 0, 0)); + + this->dataPtr->node.Request("/world/" + this->dataPtr->worldName + "/create", + req, cb); +} + +///////////////////////////////////////////////// +QString Spawn::ErrorPopupText() const +{ + return this->dataPtr->errorPopupText; +} + +///////////////////////////////////////////////// +void Spawn::SetErrorPopupText(const QString &_errorTxt) +{ + this->dataPtr->errorPopupText = _errorTxt; + this->ErrorPopupTextChanged(); + this->popupError(); +} + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::Spawn, + ignition::gui::Plugin) diff --git a/src/gui/plugins/spawn/Spawn.hh b/src/gui/plugins/spawn/Spawn.hh new file mode 100644 index 00000000000..f87ec4c160e --- /dev/null +++ b/src/gui/plugins/spawn/Spawn.hh @@ -0,0 +1,85 @@ +/* + * 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_SPAWN_HH_ +#define IGNITION_GAZEBO_GUI_SPAWN_HH_ + +#include + +#include +#include + +namespace ignition +{ +namespace gazebo +{ + class SpawnPrivate; + + /// \brief Allows to spawn models and lights using the spawn gui events or + /// drag and drop. + class Spawn : public ignition::gui::Plugin + { + Q_OBJECT + + /// \brief Text for popup error + Q_PROPERTY( + QString errorPopupText + READ ErrorPopupText + WRITE SetErrorPopupText + NOTIFY ErrorPopupTextChanged + ) + + /// \brief Constructor + public: Spawn(); + + /// \brief Destructor + public: ~Spawn() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \brief Handle drop events. + /// \param[in] _event Event with drop information. + public: void OnDropped(const ignition::gui::events::DropOnScene *_event); + + /// \brief Get the text for the popup error message + /// \return The error text + public: Q_INVOKABLE QString ErrorPopupText() const; + + /// \brief Set the text for the popup error message + /// \param[in] _errorTxt The error text + public: Q_INVOKABLE void SetErrorPopupText(const QString &_errorTxt); + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + /// \brief Notify the popup error text has changed + signals: void ErrorPopupTextChanged(); + + /// \brief Notify that an error has occurred (opens popup) + /// Note that the function name needs to start with lowercase in order for + /// the connection to work on the QML side + signals: void popupError(); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/spawn/Spawn.qml b/src/gui/plugins/spawn/Spawn.qml new file mode 100644 index 00000000000..6f5999a000a --- /dev/null +++ b/src/gui/plugins/spawn/Spawn.qml @@ -0,0 +1,48 @@ +/* + * 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 2.2 +import QtQuick.Dialogs 1.0 +import QtQuick.Layouts 1.3 + +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 + + Connections { + target: Spawn + onPopupError: errorPopup.open() + } + + Dialog { + id: errorPopup + parent: ApplicationWindow.overlay + modal: true + focus: true + width: 500 + height: 200 + x: (parent.width - width) / 2 + y: (parent.height - height) / 2 + title: "Error" + Text { + text: Spawn.errorPopupText + } + standardButtons: Dialog.Ok + } +} diff --git a/src/gui/plugins/spawn/Spawn.qrc b/src/gui/plugins/spawn/Spawn.qrc new file mode 100644 index 00000000000..bbdcea6f132 --- /dev/null +++ b/src/gui/plugins/spawn/Spawn.qrc @@ -0,0 +1,5 @@ + + + Spawn.qml + + diff --git a/src/gui/plugins/tape_measure/CMakeLists.txt b/src/gui/plugins/tape_measure/CMakeLists.txt deleted file mode 100644 index 4b5322fc2a4..00000000000 --- a/src/gui/plugins/tape_measure/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -gz_add_gui_plugin(TapeMeasure - SOURCES TapeMeasure.cc - QT_HEADERS TapeMeasure.hh - PRIVATE_LINK_LIBS - ${IGNITION-RENDERING_LIBRARIES} -) diff --git a/src/gui/plugins/tape_measure/TapeMeasure.cc b/src/gui/plugins/tape_measure/TapeMeasure.cc deleted file mode 100644 index 39c4bf02480..00000000000 --- a/src/gui/plugins/tape_measure/TapeMeasure.cc +++ /dev/null @@ -1,333 +0,0 @@ -/* - * 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 "ignition/gazebo/gui/GuiEvents.hh" -#include "TapeMeasure.hh" - -namespace ignition::gazebo -{ - class TapeMeasurePrivate - { - /// \brief Ignition communication node. - public: transport::Node node; - - /// \brief True if currently measuring, else false. - public: bool measure = false; - - /// \brief The id of the start point marker. - public: const int kStartPointId = 1; - - /// \brief The id of the end point marker. - public: const int kEndPointId = 2; - - /// \brief The id of the line marker. - public: const int kLineId = 3; - - /// \brief The id of the start or end point marker that is currently - /// being placed. This is primarily used to track the state machine of - /// the plugin. - public: int currentId = kStartPointId; - - /// \brief The location of the placed starting point of the tape measure - /// tool, only set when the user clicks to set the point. - public: ignition::math::Vector3d startPoint = - ignition::math::Vector3d::Zero; - - /// \brief The location of the placed ending point of the tape measure - /// tool, only set when the user clicks to set the point. - public: ignition::math::Vector3d endPoint = ignition::math::Vector3d::Zero; - - /// \brief The color to set the marker when hovering the mouse over the - /// scene. - public: ignition::math::Color - hoverColor{ignition::math::Color(0.2f, 0.2f, 0.2f, 0.5f)}; - - /// \brief The color to draw the marker when the user clicks to confirm - /// its location. - public: ignition::math::Color - drawColor{ignition::math::Color(0.2f, 0.2f, 0.2f, 1.0f)}; - - /// \brief A set of the currently placed markers. Used to make sure a - /// non-existent marker is not deleted. - public: std::unordered_set placedMarkers; - - /// \brief The current distance between the two points. This distance - /// is updated as the user hovers the end point as well. - public: double distance = 0.0; - - /// \brief The namespace that the markers for this plugin are placed in. - public: std::string ns = "tape_measure"; - }; -} - -using namespace ignition; -using namespace gazebo; - -///////////////////////////////////////////////// -TapeMeasure::TapeMeasure() - : ignition::gui::Plugin(), - dataPtr(std::make_unique()) -{ -} - -///////////////////////////////////////////////// -TapeMeasure::~TapeMeasure() = default; - -///////////////////////////////////////////////// -void TapeMeasure::LoadConfig(const tinyxml2::XMLElement *) -{ - if (this->title.empty()) - this->title = "Tape measure"; - - ignition::gui::App()->findChild - ()->installEventFilter(this); - ignition::gui::App()->findChild - ()->QuickWindow()->installEventFilter(this); -} - -///////////////////////////////////////////////// -void TapeMeasure::OnMeasure() -{ - this->Measure(); -} - -///////////////////////////////////////////////// -void TapeMeasure::Measure() -{ - this->Reset(); - this->dataPtr->measure = true; - QGuiApplication::setOverrideCursor(Qt::CrossCursor); - - // Notify Scene3D to disable the right click menu while we use it to - // cancel our current measuring action - ignition::gui::events::DropdownMenuEnabled dropdownMenuEnabledEvent(false); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &dropdownMenuEnabledEvent); -} - -///////////////////////////////////////////////// -void TapeMeasure::OnReset() -{ - this->Reset(); -} - -///////////////////////////////////////////////// -void TapeMeasure::Reset() -{ - this->DeleteMarker(this->dataPtr->kStartPointId); - this->DeleteMarker(this->dataPtr->kEndPointId); - this->DeleteMarker(this->dataPtr->kLineId); - - this->dataPtr->currentId = this->dataPtr->kStartPointId; - this->dataPtr->startPoint = ignition::math::Vector3d::Zero; - this->dataPtr->endPoint = ignition::math::Vector3d::Zero; - this->dataPtr->distance = 0.0; - this->dataPtr->measure = false; - this->newDistance(); - QGuiApplication::restoreOverrideCursor(); - - // Notify Scene3D that we are done using the right click, so it can - // re-enable the settings menu - ignition::gui::events::DropdownMenuEnabled dropdownMenuEnabledEvent(true); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &dropdownMenuEnabledEvent); -} - -///////////////////////////////////////////////// -double TapeMeasure::Distance() -{ - return this->dataPtr->distance; -} - -///////////////////////////////////////////////// -void TapeMeasure::DeleteMarker(int _id) -{ - if (this->dataPtr->placedMarkers.find(_id) == - this->dataPtr->placedMarkers.end()) - return; - - // Delete the previously created marker - ignition::msgs::Marker markerMsg; - markerMsg.set_ns(this->dataPtr->ns); - markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::DELETE_MARKER); - this->dataPtr->node.Request("/marker", markerMsg); - this->dataPtr->placedMarkers.erase(_id); -} - -///////////////////////////////////////////////// -void TapeMeasure::DrawPoint(int _id, - ignition::math::Vector3d &_point, ignition::math::Color &_color) -{ - this->DeleteMarker(_id); - - ignition::msgs::Marker markerMsg; - markerMsg.set_ns(this->dataPtr->ns); - markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::SPHERE); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); - ignition::msgs::Set(markerMsg.mutable_scale(), - ignition::math::Vector3d(0.1, 0.1, 0.1)); - ignition::msgs::Set(markerMsg.mutable_pose(), - ignition::math::Pose3d(_point.X(), _point.Y(), _point.Z(), 0, 0, 0)); - - this->dataPtr->node.Request("/marker", markerMsg); - this->dataPtr->placedMarkers.insert(_id); -} - -///////////////////////////////////////////////// -void TapeMeasure::DrawLine(int _id, ignition::math::Vector3d &_startPoint, - ignition::math::Vector3d &_endPoint, ignition::math::Color &_color) -{ - this->DeleteMarker(_id); - - ignition::msgs::Marker markerMsg; - markerMsg.set_ns(this->dataPtr->ns); - markerMsg.set_id(_id); - markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); - markerMsg.set_type(ignition::msgs::Marker::LINE_LIST); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_ambient(), _color); - ignition::msgs::Set(markerMsg.mutable_material()->mutable_diffuse(), _color); - ignition::msgs::Set(markerMsg.add_point(), _startPoint); - ignition::msgs::Set(markerMsg.add_point(), _endPoint); - - this->dataPtr->node.Request("/marker", markerMsg); - this->dataPtr->placedMarkers.insert(_id); -} - -///////////////////////////////////////////////// -bool TapeMeasure::eventFilter(QObject *_obj, QEvent *_event) -{ - if (_event->type() == ignition::gui::events::HoverToScene::kType) - { - auto hoverToSceneEvent = - reinterpret_cast(_event); - - // This event is called in Scene3d's RenderThread, so it's safe to make - // rendering calls here - if (this->dataPtr->measure && hoverToSceneEvent) - { - ignition::math::Vector3d point = hoverToSceneEvent->Point(); - this->DrawPoint(this->dataPtr->currentId, point, - this->dataPtr->hoverColor); - - // If the user is currently choosing the end point, draw the connecting - // line and update the new distance. - if (this->dataPtr->currentId == this->dataPtr->kEndPointId) - { - this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint, - point, this->dataPtr->hoverColor); - this->dataPtr->distance = this->dataPtr->startPoint.Distance(point); - this->newDistance(); - } - } - } - else if (_event->type() == ignition::gui::events::LeftClickToScene::kType) - { - auto leftClickToSceneEvent = - reinterpret_cast(_event); - - // This event is called in Scene3d's RenderThread, so it's safe to make - // rendering calls here - if (this->dataPtr->measure && leftClickToSceneEvent) - { - ignition::math::Vector3d point = leftClickToSceneEvent->Point(); - this->DrawPoint(this->dataPtr->currentId, point, - this->dataPtr->drawColor); - // If the user is placing the start point, update its position - if (this->dataPtr->currentId == this->dataPtr->kStartPointId) - { - this->dataPtr->startPoint = point; - } - // If the user is placing the end point, update the end position, - // end the measurement state, and update the draw line and distance - else - { - this->dataPtr->endPoint = point; - this->dataPtr->measure = false; - this->DrawLine(this->dataPtr->kLineId, this->dataPtr->startPoint, - this->dataPtr->endPoint, this->dataPtr->drawColor); - this->dataPtr->distance = - this->dataPtr->startPoint.Distance(this->dataPtr->endPoint); - this->newDistance(); - QGuiApplication::restoreOverrideCursor(); - - // Notify Scene3D that we are done using the right click, so it can - // re-enable the settings menu - ignition::gui::events::DropdownMenuEnabled - dropdownMenuEnabledEvent(true); - - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &dropdownMenuEnabledEvent); - } - this->dataPtr->currentId = this->dataPtr->kEndPointId; - } - } - else if (_event->type() == QEvent::KeyPress) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent && keyEvent->key() == Qt::Key_M) - { - this->Reset(); - this->Measure(); - } - } - else if (_event->type() == QEvent::KeyRelease) - { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent && keyEvent->key() == Qt::Key_Escape && - this->dataPtr->measure) - { - this->Reset(); - } - } - // Cancel the current action if a right click is detected - else if (_event->type() == ignition::gui::events::RightClickToScene::kType) - { - if (this->dataPtr->measure) - { - this->Reset(); - } - } - - return QObject::eventFilter(_obj, _event); -} - -// Register this plugin -IGNITION_ADD_PLUGIN(ignition::gazebo::TapeMeasure, - ignition::gui::Plugin) diff --git a/src/gui/plugins/tape_measure/TapeMeasure.hh b/src/gui/plugins/tape_measure/TapeMeasure.hh deleted file mode 100644 index 0da8719f2b0..00000000000 --- a/src/gui/plugins/tape_measure/TapeMeasure.hh +++ /dev/null @@ -1,105 +0,0 @@ -/* - * 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_GUI_TAPEMEASURE_HH_ -#define IGNITION_GAZEBO_GUI_TAPEMEASURE_HH_ - -#include - -#include -#include -#include - -namespace ignition -{ -namespace gazebo -{ - class TapeMeasurePrivate; - - /// \brief Provides buttons for the tape measure tool. - class TapeMeasure : public ignition::gui::Plugin - { - Q_OBJECT - - /// \brief Constructor - public: TapeMeasure(); - - /// \brief Destructor - public: ~TapeMeasure() override; - - // Documentation inherited - public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; - - /// \brief Deletes the marker with the provided id within the - /// "tape_measure" namespace. - /// \param[in] _id The id of the marker - public: void DeleteMarker(int _id); - - /// \brief Resets all of the relevant data for this plugin. Called when - /// the user clicks the reset button and when the user starts a new - /// measurement. - public: void Reset(); - - /// \brief Starts a new measurement. Erases any previous measurement in - /// progress or already made. - public: void Measure(); - - /// \brief Draws a point marker. Called to display the start and end - /// point of the tape measure. - /// \param[in] _id The id of the marker - /// \param[in] _point The x, y, z coordinates of where to place the marker - /// \param[in] _color The rgba color to set the marker - public: void DrawPoint(int _id, - ignition::math::Vector3d &_point, - ignition::math::Color &_color); - - /// \brief Draws a line marker. Called to display the line between the - /// start and end point of the tape measure. - /// \param[in] _id The id of the marker - /// \param[in] _startPoint The x, y, z coordinates of the line start point - /// \param[in] _endPoint The x, y, z coordinates of the line end point - /// \param[in] _color The rgba color to set the marker - public: void DrawLine(int _id, - ignition::math::Vector3d &_startPoint, - ignition::math::Vector3d &_endPoint, - ignition::math::Color &_color); - - /// \brief Callback in Qt thread when the new measurement button is - /// clicked. - public slots: void OnMeasure(); - - /// \brief Callback in Qt thread when the reset button is clicked. - public slots: void OnReset(); - - /// \brief Callback in Qt thread to get the distance to display in the - /// gui window. - /// \return The distance between the start and end point of the measurement - public slots: double Distance(); - - // Documentation inherited - protected: bool eventFilter(QObject *_obj, QEvent *_event) override; - - /// \brief Signal fired when a new tape measure distance is set. - signals: void newDistance(); - - /// \internal - /// \brief Pointer to private data. - private: std::unique_ptr dataPtr; - }; -} -} - -#endif diff --git a/src/gui/plugins/tape_measure/TapeMeasure.qrc b/src/gui/plugins/tape_measure/TapeMeasure.qrc deleted file mode 100644 index e87212d2760..00000000000 --- a/src/gui/plugins/tape_measure/TapeMeasure.qrc +++ /dev/null @@ -1,7 +0,0 @@ - - - TapeMeasure.qml - tape_measure.png - trashcan.png - - diff --git a/src/gui/plugins/tape_measure/tape_measure.png b/src/gui/plugins/tape_measure/tape_measure.png deleted file mode 100644 index 228e5834424..00000000000 Binary files a/src/gui/plugins/tape_measure/tape_measure.png and /dev/null differ diff --git a/src/gui/plugins/tape_measure/trashcan.png b/src/gui/plugins/tape_measure/trashcan.png deleted file mode 100644 index 62edd38a519..00000000000 Binary files a/src/gui/plugins/tape_measure/trashcan.png and /dev/null differ diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index c9bfe50636d..54215f83049 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -20,12 +20,18 @@ #include #include +#include #include +#include #include +#include #include +#include +#include #include #include +#include #include #include #include @@ -34,10 +40,10 @@ #include #include #include +#include #include #include #include -#include #include "ignition/gazebo/gui/GuiEvents.hh" @@ -45,14 +51,56 @@ namespace ignition::gazebo { class TransformControlPrivate { + /// \brief Perform transformations in the render thread. + public: void HandleTransform(); + + /// \brief Snaps a point at intervals of a fixed distance. Currently used + /// to give a snapping behavior when moving models with a mouse. + /// \param[in] _point Input point to snap. + /// \param[in] _snapVals The snapping values to use for each corresponding + /// coordinate in _point + /// \param[in] _sensitivity Sensitivity of a point snapping, in terms of a + /// percentage of the interval. + public: void SnapPoint( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity = 0.4) const; + + /// \brief Constraints the passed in axis to the currently selected axes. + /// \param[in, out] _axis The axis to constrain. + public: void XYZConstraint(math::Vector3d &_axis); + + /// \brief Snaps a value at intervals of a fixed distance. Currently used + /// to give a snapping behavior when moving models with a mouse. + /// \param[in] _coord Input coordinate point. + /// \param[in] _interval Fixed distance interval at which the point is + /// snapped. + /// \param[in] _sensitivity Sensitivity of a point snapping, in terms of a + /// percentage of the interval. + /// \return Snapped coordinate point. + public: double SnapValue( + double _coord, double _interval, double _sensitivity) const; + + /// \brief Get the top level node for the given node, which + /// is the ancestor which is a direct child to the root visual. + /// Usually, this will be a model or a light. + /// \param[in] _node Child node + /// \return Top level node containining this node + rendering::NodePtr TopLevelNode( + const rendering::NodePtr &_node) const; + /// \brief Ignition communication node. public: transport::Node node; /// \brief Mutex to protect mode + // TODO(anyone): check on mutex usage public: std::mutex mutex; /// \brief Transform control service name - public: std::string service; + /// Only used when in legacy mode, where this plugin requests a + /// transport service provided by `GzScene3D`. + /// The new behaviour is that this plugin performs the entire transform + /// operation. + public: std::string service{"/gui/transform_mode"}; /// \brief Flag for if the snapping values should be set to the grid. public: bool snapToGrid{false}; @@ -68,6 +116,69 @@ namespace ignition::gazebo /// \brief The scale snap values held for snap to grid. public: math::Vector3d scaleSnapVals{1.0, 1.0, 1.0}; + + /// \brief Transform mode: none, translation, rotation, or scale + public: rendering::TransformMode transformMode = + rendering::TransformMode::TM_NONE; + + /// \brief Transform space: local or world + public: rendering::TransformSpace transformSpace = + rendering::TransformSpace::TS_LOCAL; + + /// \brief Transform controller for models + public: rendering::TransformController transformControl; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief True if there are new mouse events to process. + public: bool mouseDirty{false}; + + /// \brief Whether the transform gizmo is being dragged. + public: bool transformActive{false}; + + /// \brief Name of service for setting entity pose + public: std::string poseCmdService; + + /// \brief Currently selected entities, organized by order of selection. + public: std::vector selectedEntities; + + /// \brief Holds the latest mouse event + public: ignition::common::MouseEvent mouseEvent; + + /// \brief Holds the latest key event + public: ignition::common::KeyEvent keyEvent; + + /// \brief Flag to indicate whether the x key is currently being pressed + public: bool xPressed = false; + + /// \brief Flag to indicate whether the y key is currently being pressed + public: bool yPressed = false; + + /// \brief Flag to indicate whether the z key is currently being pressed + public: bool zPressed = false; + + /// \brief Where the mouse left off - used to continue translating + /// smoothly when switching axes through keybinding and clicking + /// Updated on an x, y, or z, press or release and a mouse press + public: math::Vector2i mousePressPos = math::Vector2i::Zero; + + /// \brief Flag to keep track of world pose setting used + /// for button translating. + public: bool isStartWorldPosSet = false; + + /// \brief The starting world pose of a clicked visual. + public: ignition::math::Vector3d startWorldPos = math::Vector3d::Zero; + + /// \brief Block orbit + public: bool blockOrbit = false; + + /// \brief Enable legacy features for plugin to work with GzScene3D. + /// Disable them to work with the new MinimalScene plugin. + public: bool legacy{false}; }; } @@ -85,13 +196,29 @@ TransformControl::TransformControl() TransformControl::~TransformControl() = default; ///////////////////////////////////////////////// -void TransformControl::LoadConfig(const tinyxml2::XMLElement *) +void TransformControl::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { if (this->title.empty()) this->title = "Transform control"; - // For transform requests - this->dataPtr->service = "/gui/transform_mode"; + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("legacy")) + { + elem->QueryBoolText(&this->dataPtr->legacy); + } + } + + if (this->dataPtr->legacy) + { + igndbg << "Legacy mode is enabled; this plugin must be used with " + << "GzScene3D." << std::endl; + } + else + { + igndbg << "Legacy mode is disabled; this plugin must be used with " + << "MinimalScene." << std::endl; + } ignition::gui::App()->findChild ()->installEventFilter(this); @@ -109,22 +236,16 @@ void TransformControl::OnSnapUpdate( this->dataPtr->rpySnapVals = math::Vector3d(_roll, _pitch, _yaw); this->dataPtr->scaleSnapVals = math::Vector3d(_scaleX, _scaleY, _scaleZ); - ignition::gui::events::SnapIntervals event( - this->dataPtr->xyzSnapVals, - this->dataPtr->rpySnapVals, - this->dataPtr->scaleSnapVals); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), &event); - - IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION - ignition::gui::events::SnapIntervals oldEvent( - this->dataPtr->xyzSnapVals, - this->dataPtr->rpySnapVals, - this->dataPtr->scaleSnapVals); - ignition::gui::App()->sendEvent( - ignition::gui::App()->findChild(), - &oldEvent); - IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION + // Emit event to GzScene3D in legacy mode + if (this->dataPtr->legacy) + { + ignition::gui::events::SnapIntervals event( + this->dataPtr->xyzSnapVals, + this->dataPtr->rpySnapVals, + this->dataPtr->scaleSnapVals); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), &event); + } this->newSnapValues(); } @@ -132,16 +253,44 @@ void TransformControl::OnSnapUpdate( ///////////////////////////////////////////////// void TransformControl::OnMode(const QString &_mode) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + auto modeStr = _mode.toStdString(); + + // Legacy behaviour: send request to GzScene3D + if (this->dataPtr->legacy) { - if (!_result) - ignerr << "Error setting transform mode" << std::endl; - }; + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting transform mode" << std::endl; + }; - ignition::msgs::StringMsg req; - req.set_data(_mode.toStdString()); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + ignition::msgs::StringMsg req; + req.set_data(modeStr); + this->dataPtr->node.Request(this->dataPtr->service, req, cb); + } + // New behaviour: handle the transform control locally + else + { + std::lock_guard lock(this->dataPtr->mutex); + if (modeStr == "select") + this->dataPtr->transformMode = rendering::TransformMode::TM_NONE; + else if (modeStr == "translate") + this->dataPtr->transformMode = rendering::TransformMode::TM_TRANSLATION; + else if (modeStr == "rotate") + this->dataPtr->transformMode = rendering::TransformMode::TM_ROTATION; + else if (modeStr == "scale") + this->dataPtr->transformMode = rendering::TransformMode::TM_SCALE; + else + ignerr << "Unknown transform mode: [" << modeStr << "]" << std::endl; + + ignition::gazebo::gui::events::TransformControlModeActive + transformControlModeActive(this->dataPtr->transformMode); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &transformControlModeActive); + this->dataPtr->mouseDirty = true; + } } ///////////////////////////////////////////////// @@ -207,28 +356,99 @@ bool TransformControl::eventFilter(QObject *_obj, QEvent *_event) this->SnapToGrid(); this->dataPtr->snapToGrid = false; } + if (this->dataPtr->transformControl.Active()) + this->dataPtr->mouseDirty = true; + this->dataPtr->HandleTransform(); + } + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) + { + if (!this->dataPtr->blockOrbit) + { + ignition::gazebo::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntities = _e->Data(); + } } - else if (_event->type() == QEvent::KeyPress) + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_T) + if (!this->dataPtr->blockOrbit) + { + this->dataPtr->selectedEntities.clear(); + } + } + else if (_event->type() == ignition::gui::events::LeftClickOnScene::kType) + { + ignition::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::MousePressOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::DragOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == ignition::gui::events::KeyPressOnScene::kType) + { + ignition::gui::events::KeyPressOnScene *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); + + if (this->dataPtr->keyEvent.Key() == Qt::Key_T) { this->activateTranslate(); } - else if (keyEvent->key() == Qt::Key_R) + else if (this->dataPtr->keyEvent.Key() == Qt::Key_R) { this->activateRotate(); } } - else if (_event->type() == QEvent::KeyRelease) + else if (_event->type() == ignition::gui::events::KeyReleaseOnScene::kType) { - QKeyEvent *keyEvent = static_cast(_event); - if (keyEvent->key() == Qt::Key_Escape) + ignition::gui::events::KeyReleaseOnScene *_e = + static_cast(_event); + this->dataPtr->keyEvent = _e->Key(); + if (this->dataPtr->keyEvent.Key() == Qt::Key_Escape) { this->activateSelect(); } } + if (this->dataPtr->legacy) + { + if (_event->type() == QEvent::KeyPress) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent->key() == Qt::Key_T) + { + this->activateTranslate(); + } + else if (keyEvent->key() == Qt::Key_R) + { + this->activateRotate(); + } + } + else if (_event->type() == QEvent::KeyRelease) + { + QKeyEvent *keyEvent = static_cast(_event); + if (keyEvent->key() == Qt::Key_Escape) + { + this->activateSelect(); + } + } + } + return QObject::eventFilter(_obj, _event); } @@ -286,6 +506,516 @@ double TransformControl::scaleZSnap() return this->dataPtr->scaleSnapVals[2]; } +///////////////////////////////////////////////// +void TransformControlPrivate::HandleTransform() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "TransformControl plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->transformControl.Camera()) + this->transformControl.SetCamera(this->camera); + } + + // set transform configuration + this->transformControl.SetTransformMode(this->transformMode); + + // stop and detach transform controller if mode is none or no entity is + // selected + if (this->transformMode == rendering::TransformMode::TM_NONE || + (this->transformControl.Node() && + this->selectedEntities.empty())) + { + if (this->transformControl.Node()) + { + try + { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + + if (this->transformControl.Active()) + this->transformControl.Stop(); + + this->transformControl.Detach(); + } + else + { + // shift indicates world space transformation + this->transformSpace = (this->keyEvent.Shift()) ? + rendering::TransformSpace::TS_WORLD : + rendering::TransformSpace::TS_LOCAL; + this->transformControl.SetTransformSpace( + this->transformSpace); + } + + // update gizmo visual + this->transformControl.Update(); + + // check for mouse events + if (!this->mouseDirty) + return; + + // handle mouse movements + if (this->mouseEvent.Button() == ignition::common::MouseEvent::LEFT) + { + if (this->mouseEvent.Type() == ignition::common::MouseEvent::PRESS + && this->transformControl.Node()) + { + this->mousePressPos = this->mouseEvent.Pos(); + + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (visual) + { + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = + this->transformControl.AxisById(visual->Id()); + if (axis != ignition::math::Vector3d::Zero) + { + this->blockOrbit = true; + // start the transform process + this->transformControl.SetActiveAxis(axis); + this->transformControl.Start(); + if (this->transformControl.Node()) + { + try + { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + this->mouseDirty = false; + } + else + { + this->blockOrbit = false; + return; + } + } + } + else if (this->mouseEvent.Type() == ignition::common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + + this->isStartWorldPosSet = false; + if (this->transformControl.Active()) + { + if (this->transformControl.Node()) + { + std::function cb = + [this](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (this->transformControl.Node()) + { + try + { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + if (!_result) + ignerr << "Error setting pose" << std::endl; + }; + rendering::NodePtr nodeTmp = this->transformControl.Node(); + auto topVisual = std::dynamic_pointer_cast( + nodeTmp); + ignition::msgs::Pose req; + req.set_name(topVisual->Name()); + msgs::Set(req.mutable_position(), nodeTmp->WorldPosition()); + msgs::Set(req.mutable_orientation(), nodeTmp->WorldRotation()); + + // First time, create the service + if (this->poseCmdService.empty()) + { + std::string worldName; + auto worldNames = ignition::gui::worldNames(); + if (!worldNames.empty()) + worldName = worldNames[0].toStdString(); + + this->poseCmdService = "/world/" + worldName + "/set_pose"; + + this->poseCmdService = transport::TopicUtils::AsValidTopic( + this->poseCmdService); + if (this->poseCmdService.empty()) + { + ignerr << "Failed to create valid pose command service " + << "for world [" << worldName << "]" << std::endl; + return; + } + } + this->node.Request(this->poseCmdService, req, cb); + } + + this->transformControl.Stop(); + this->mouseDirty = false; + } + // Select entity + else if (!this->mouseEvent.Dragging()) + { + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual) + { + return; + } + + // check if the visual is an axis in the gizmo visual + math::Vector3d axis = this->transformControl.AxisById(visual->Id()); + if (axis == ignition::math::Vector3d::Zero) + { + auto topNode = this->TopLevelNode(visual); + if (!topNode) + { + return; + } + + auto topVis = std::dynamic_pointer_cast(topNode); + // TODO(anyone) Check plane geometry instead of hardcoded name! + if (topVis && topVis->Name() != "ground_plane") + { + // Highlight entity and notify other widgets + + // Attach control if in a transform mode - control is attached to: + // * latest selection + // * top-level nodes (model, light...) + if (this->transformMode != rendering::TransformMode::TM_NONE) + { + rendering::VisualPtr clickedVisual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + auto topClickedNode = this->TopLevelNode(clickedVisual); + auto topClickedVisual = + std::dynamic_pointer_cast(topClickedNode); + + if (topClickedNode == topClickedVisual) + { + this->transformControl.Attach(topClickedVisual); + try + { + topClickedVisual->SetUserData( + "pause-update", static_cast(1)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + else + { + this->transformControl.Detach(); + try + { + topClickedVisual->SetUserData( + "pause-update", static_cast(0)); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + } + + this->mouseDirty = false; + return; + } + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->transformControl.Active()) + { + if (this->transformControl.Node()){ + try + { + this->transformControl.Node()->SetUserData( + "pause-update", static_cast(1)); + } catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + + this->blockOrbit = true; + // compute the the start and end mouse positions in normalized coordinates + auto imageWidth = static_cast(this->camera->ImageWidth()); + auto imageHeight = static_cast( + this->camera->ImageHeight()); + double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; + double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; + double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; + double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; + math::Vector2d start(nx, ny); + math::Vector2d end(nxEnd, nyEnd); + + // get the current active axis + math::Vector3d axis = this->transformControl.ActiveAxis(); + + // compute 3d transformation from 2d mouse movement + if (this->transformControl.Mode() == + rendering::TransformMode::TM_TRANSLATION) + { + Entity nodeId = this->selectedEntities.front(); + rendering::NodePtr target; + for (unsigned int i = 0; i < this->scene->VisualCount(); i++) + { + auto visual = this->scene->VisualByIndex(i); + auto entityId = kNullEntity; + try + { + entityId = static_cast( + std::get(visual->UserData("gazebo-entity"))); + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + if (entityId == nodeId) + { + target = std::dynamic_pointer_cast( + this->scene->VisualById(visual->Id())); + break; + } + } + if (!target) + { + ignwarn << "Failed to find node with ID [" << nodeId << "]" + << std::endl; + return; + } + this->XYZConstraint(axis); + if (!this->isStartWorldPosSet) + { + this->isStartWorldPosSet = true; + this->startWorldPos = target->WorldPosition(); + } + ignition::math::Vector3d worldPos = target->WorldPosition(); + math::Vector3d distance = + this->transformControl.TranslationFrom2d(axis, start, end); + if (this->keyEvent.Control()) + { + // Translate to world frame for snapping + distance += this->startWorldPos; + math::Vector3d snapVals = this->xyzSnapVals; + + // Constrain snap values to a minimum of 1e-4 + snapVals.X() = std::max(1e-4, snapVals.X()); + snapVals.Y() = std::max(1e-4, snapVals.Y()); + snapVals.Z() = std::max(1e-4, snapVals.Z()); + + this->SnapPoint(distance, snapVals); + + // Translate back to entity frame + distance -= this->startWorldPos; + distance *= axis; + } + this->transformControl.Translate(distance); + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_ROTATION) + { + math::Quaterniond rotation = + this->transformControl.RotationFrom2d(axis, start, end); + + if (this->keyEvent.Control()) + { + math::Vector3d currentRot = rotation.Euler(); + math::Vector3d snapVals = this->rpySnapVals; + + if (snapVals.X() <= 1e-4) + { + snapVals.X() = IGN_PI/4; + } + else + { + snapVals.X() = IGN_DTOR(snapVals.X()); + } + if (snapVals.Y() <= 1e-4) + { + snapVals.Y() = IGN_PI/4; + } + else + { + snapVals.Y() = IGN_DTOR(snapVals.Y()); + } + if (snapVals.Z() <= 1e-4) + { + snapVals.Z() = IGN_PI/4; + } + else + { + snapVals.Z() = IGN_DTOR(snapVals.Z()); + } + + this->SnapPoint(currentRot, snapVals); + rotation = math::Quaterniond::EulerToQuaternion(currentRot); + } + this->transformControl.Rotate(rotation); + } + else if (this->transformControl.Mode() == + rendering::TransformMode::TM_SCALE) + { + this->XYZConstraint(axis); + // note: scaling is limited to local space + math::Vector3d scale = + this->transformControl.ScaleFrom2d(axis, start, end); + if (this->keyEvent.Control()) + { + math::Vector3d snapVals = this->scaleSnapVals; + + if (snapVals.X() <= 1e-4) + snapVals.X() = 0.1; + if (snapVals.Y() <= 1e-4) + snapVals.Y() = 0.1; + if (snapVals.Z() <= 1e-4) + snapVals.Z() = 0.1; + + this->SnapPoint(scale, snapVals); + } + this->transformControl.Scale(scale); + } + this->mouseDirty = false; + } + + ignition::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + ignition::gui::App()->sendEvent( + ignition::gui::App()->findChild(), + &blockOrbitEvent); +} + +///////////////////////////////////////////////// +rendering::NodePtr TransformControlPrivate::TopLevelNode( + const rendering::NodePtr &_node) const +{ + if (!this->scene) + return rendering::NodePtr(); + + rendering::NodePtr rootNode = this->scene->RootVisual(); + + rendering::NodePtr nodeTmp = _node; + while (nodeTmp && nodeTmp->Parent() != rootNode) + { + nodeTmp = + std::dynamic_pointer_cast(nodeTmp->Parent()); + } + + return nodeTmp; +} + +///////////////////////////////////////////////// +double TransformControlPrivate::SnapValue( + double _coord, double _interval, double _sensitivity) const +{ + double snap = _interval * _sensitivity; + double rem = fmod(_coord, _interval); + double minInterval = _coord - rem; + + if (rem < 0) + { + minInterval -= _interval; + } + + double maxInterval = minInterval + _interval; + + if (_coord < (minInterval + snap)) + { + _coord = minInterval; + } + else if (_coord > (maxInterval - snap)) + { + _coord = maxInterval; + } + + return _coord; +} + +///////////////////////////////////////////////// +void TransformControlPrivate::XYZConstraint(math::Vector3d &_axis) +{ + math::Vector3d translationAxis = math::Vector3d::Zero; + + if (this->xPressed) + { + translationAxis += math::Vector3d::UnitX; + } + + if (this->yPressed) + { + translationAxis += math::Vector3d::UnitY; + } + + if (this->zPressed) + { + translationAxis += math::Vector3d::UnitZ; + } + + if (translationAxis != math::Vector3d::Zero) + { + _axis = translationAxis; + } +} + +///////////////////////////////////////////////// +void TransformControlPrivate::SnapPoint( + ignition::math::Vector3d &_point, math::Vector3d &_snapVals, + double _sensitivity) const +{ + if (_snapVals.X() <= 0 || _snapVals.Y() <= 0 || _snapVals.Z() <= 0) + { + ignerr << "Interval distance must be greater than 0" + << std::endl; + return; + } + + if (_sensitivity < 0 || _sensitivity > 1.0) + { + ignerr << "Sensitivity must be between 0 and 1" << std::endl; + return; + } + + _point.X() = this->SnapValue(_point.X(), _snapVals.X(), _sensitivity); + _point.Y() = this->SnapValue(_point.Y(), _snapVals.Y(), _sensitivity); + _point.Z() = this->SnapValue(_point.Z(), _snapVals.Z(), _sensitivity); +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::TransformControl, ignition::gui::Plugin) diff --git a/src/gui/plugins/video_recorder/CMakeLists.txt b/src/gui/plugins/video_recorder/CMakeLists.txt index 93964b3490b..bf721fc2f60 100644 --- a/src/gui/plugins/video_recorder/CMakeLists.txt +++ b/src/gui/plugins/video_recorder/CMakeLists.txt @@ -1,4 +1,8 @@ gz_add_gui_plugin(VideoRecorder - SOURCES VideoRecorder.cc - QT_HEADERS VideoRecorder.hh + SOURCES + VideoRecorder.cc + QT_HEADERS + VideoRecorder.hh + PUBLIC_LINK_LIBS + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ) diff --git a/src/gui/plugins/video_recorder/VideoRecorder.cc b/src/gui/plugins/video_recorder/VideoRecorder.cc index 84406d718f9..1eb9b3e69a5 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.cc +++ b/src/gui/plugins/video_recorder/VideoRecorder.cc @@ -25,81 +25,400 @@ #include #include +#include +#include #include +#include +#include #include +#include +#include +#include #include #include +/// \brief condition variable for lockstepping video recording +/// todo(anyone) avoid using a global condition variable when we support +/// multiple viewports in the future. +std::condition_variable g_renderCv; + namespace ignition::gazebo { class VideoRecorderPrivate { + /// \brief Capture a video frame in the render thread. + public: void OnRender(); + + /// \brief Initialize rendering and transport. + public: void Initialize(); + /// \brief Ignition communication node. public: transport::Node node; - /// \brief Video record service name - public: std::string service; + /// \brief Pointer to the camera being recorded + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Pointer to the 3D scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief Video encoder + public: common::VideoEncoder videoEncoder; + + /// \brief Image from user camera + public: rendering::Image cameraImage; + + /// \brief True to record a video from the user camera + public: bool recordVideo = false; + + /// \brief Video encoding format + public: std::string format; + + /// \brief Use sim time as timestamp during video recording + /// By default (false), video encoding is done using real time. + public: bool useSimTime = false; + + /// \brief Lockstep gui with ECM when recording + public: bool lockstep = false; + + /// \brief Video recorder bitrate (bps) + public: unsigned int bitrate = 2070000; + + /// \brief Start tiem of video recording + public: std::chrono::steady_clock::time_point startTime; + + /// \brief Camera pose publisher + public: transport::Node::Publisher recorderStatsPub; + + /// \brief Record stats topic name + public: std::string recorderStatsTopic = "/gui/record_video/stats"; + + /// \brief Record video service + /// Only used when in legacy mode, where this plugin requests a + /// transport service provided by `GzScene3D`. + /// The new behaviour is that this plugin performs the entire operation. + public: std::string service = "/gui/record_video"; + + /// \brief True to indicate video recording in progress + public: bool recording = false; + + /// \brief mutex to protect the recording variable + public: std::mutex recordMutex; + + /// \brief mutex to protect the render condition variable + /// Used when recording in lockstep mode. + public: std::mutex renderMutex; + + /// \brief Total time elapsed in simulation. This will not increase while + /// paused. + public: std::chrono::steady_clock::duration simTime{0}; /// \brief Filename of the recorded video public: std::string filename; + + /// \brief Enable legacy features for plugin to work with GzScene3D. + /// Disable them to work with the new MinimalScene plugin. + public: bool legacy{false}; }; } using namespace ignition; using namespace gazebo; +///////////////////////////////////////////////// +void VideoRecorderPrivate::Initialize() +{ + // Don't setup rendering or transport in legacy mode, GzScene3D takes care of + // that + if (this->legacy) + return; + + // Already initialized + if (this->scene) + return; + + this->scene = rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + return; + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + igndbg << "Video Recorder plugin is recoding camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->camera) + { + ignerr << "Camera is not available" << std::endl; + return; + } + + // recorder stats topic + this->recorderStatsPub = + this->node.Advertise(this->recorderStatsTopic); + ignmsg << "Video recorder stats topic advertised on [" + << this->recorderStatsTopic << "]" << std::endl; +} + +///////////////////////////////////////////////// +void VideoRecorderPrivate::OnRender() +{ + // Don't render in legacy mode, GzScene3D takes care of that + if (this->legacy) + return; + + this->Initialize(); + + // record video is requested + { + IGN_PROFILE("VideoRecorder Record Video"); + if (this->recordVideo) + { + unsigned int width = this->camera->ImageWidth(); + unsigned int height = this->camera->ImageHeight(); + + if (this->cameraImage.Width() != width || + this->cameraImage.Height() != height) + { + this->cameraImage = this->camera->CreateImage(); + } + + // Video recorder is on. Add more frames to it + if (this->videoEncoder.IsEncoding()) + { + this->camera->Copy(this->cameraImage); + + std::chrono::steady_clock::time_point t = + std::chrono::steady_clock::now(); + if (this->useSimTime) + { + t = std::chrono::steady_clock::time_point( + this->simTime); + } + bool frameAdded = this->videoEncoder.AddFrame( + this->cameraImage.Data(), width, height, t); + + if (frameAdded) + { + // publish recorder stats + if (this->startTime == + std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0)))) + { + // start time, i.e. time when first frame is added + this->startTime = t; + } + + std::chrono::steady_clock::duration dt; + dt = t - this->startTime; + int64_t sec, nsec; + std::tie(sec, nsec) = ignition::math::durationToSecNsec(dt); + msgs::Time msg; + msg.set_sec(sec); + msg.set_nsec(nsec); + this->recorderStatsPub.Publish(msg); + } + } + // Video recorder is idle. Start recording. + else + { + if (this->useSimTime) + ignmsg << "Recording video using sim time." << std::endl; + if (this->lockstep) + { + ignmsg << "Recording video in lockstep mode" << std::endl; + if (!this->useSimTime) + { + ignwarn << "It is recommended to set to true " + << "when recording video in lockstep mode." << std::endl; + } + } + ignmsg << "Recording video using bitrate: " + << this->bitrate << std::endl; + this->videoEncoder.Start(this->format, + this->filename, width, height, 25, + this->bitrate); + this->startTime = std::chrono::steady_clock::time_point( + std::chrono::duration(std::chrono::seconds(0))); + } + } + else if (this->videoEncoder.IsEncoding()) + { + this->videoEncoder.Stop(); + } + } + // only has an effect in video recording lockstep mode + // this notifes ECM to continue updating the scene + g_renderCv.notify_one(); +} + ///////////////////////////////////////////////// VideoRecorder::VideoRecorder() - : gui::Plugin(), dataPtr(std::make_unique()) + : GuiSystem(), dataPtr(std::make_unique()) { } ///////////////////////////////////////////////// VideoRecorder::~VideoRecorder() = default; +////////////////////////////////////////////////// +void VideoRecorder::Update(const UpdateInfo &_info, + EntityComponentManager & /*_ecm*/) +{ + // Don't lockstep in legacy mode, GzScene3D takes care of that + if (this->dataPtr->legacy) + return; + + this->dataPtr->simTime = _info.simTime; + + // check if video recording is enabled and if we need to lock step + // ECM updates with GUI rendering during video recording + std::unique_lock lock(this->dataPtr->recordMutex); + if (this->dataPtr->recording && this->dataPtr->lockstep) + { + std::unique_lock lock2(this->dataPtr->renderMutex); + g_renderCv.wait(lock2); + } +} + ///////////////////////////////////////////////// -void VideoRecorder::LoadConfig(const tinyxml2::XMLElement *) +void VideoRecorder::LoadConfig(const tinyxml2::XMLElement * _pluginElem) { if (this->title.empty()) this->title = "Video recorder"; - // For video record requests - this->dataPtr->service = "/gui/record_video"; + // Custom parameters + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("record_video")) + { + if (auto useSimTimeElem = elem->FirstChildElement("use_sim_time")) + { + bool useSimTime = false; + if (useSimTimeElem->QueryBoolText(&useSimTime) != tinyxml2::XML_SUCCESS) + { + ignerr << "Faild to parse value: " + << useSimTimeElem->GetText() << std::endl; + } + else + { + this->dataPtr->useSimTime = useSimTime; + } + } + if (auto lockstepElem = elem->FirstChildElement("lockstep")) + { + bool lockstep = false; + if (lockstepElem->QueryBoolText(&lockstep) != tinyxml2::XML_SUCCESS) + { + ignerr << "Failed to parse value: " + << lockstepElem->GetText() << std::endl; + } + else + { + this->dataPtr->lockstep = lockstep; + } + } + if (auto bitrateElem = elem->FirstChildElement("bitrate")) + { + unsigned int bitrate = 0u; + std::stringstream bitrateStr; + bitrateStr << std::string(bitrateElem->GetText()); + bitrateStr >> bitrate; + if (bitrate > 0u) + { + this->dataPtr->bitrate = bitrate; + } + else + { + ignerr << "Video recorder bitrate must be larger than 0" + << std::endl; + } + } + } + + if (auto elem = _pluginElem->FirstChildElement("legacy")) + { + elem->QueryBoolText(&this->dataPtr->legacy); + } + } + + if (this->dataPtr->legacy) + { + igndbg << "Legacy mode is enabled; this plugin must be used with " + << "GzScene3D." << std::endl; + } + else + { + igndbg << "Legacy mode is disabled; this plugin must be used with " + << "MinimalScene." << std::endl; + } + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); } ///////////////////////////////////////////////// -void VideoRecorder::OnStart(const QString &_format) +bool VideoRecorder::eventFilter(QObject *_obj, QEvent *_event) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + if (_event->type() == ignition::gui::events::Render::kType) { - if (!_result) - ignerr << "Error sending video record start request" << std::endl; - }; + this->dataPtr->OnRender(); + } + // Standard event processing + return QObject::eventFilter(_obj, _event); +} - std::string format = _format.toStdString(); - this->dataPtr->filename = "ign_recording." + format; +///////////////////////////////////////////////// +void VideoRecorder::OnStart(const QString &_format) +{ + std::unique_lock lock(this->dataPtr->recordMutex); + this->dataPtr->format = _format.toStdString(); + this->dataPtr->filename = "ign_recording." + this->dataPtr->format; + this->dataPtr->recordVideo = true; + this->dataPtr->recording = true; - ignition::msgs::VideoRecord req; - req.set_start(true); - req.set_format(format); - req.set_save_filename(this->dataPtr->filename); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + if (this->dataPtr->legacy) + { + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending video record start request" << std::endl; + }; + ignition::msgs::VideoRecord req; + req.set_start(this->dataPtr->recordVideo); + req.set_format(this->dataPtr->format); + req.set_save_filename(this->dataPtr->filename); + this->dataPtr->node.Request(this->dataPtr->service, req, cb); + } } ///////////////////////////////////////////////// void VideoRecorder::OnStop() { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + this->dataPtr->recordVideo = false; + this->dataPtr->recording = false; + + if (this->dataPtr->legacy) { - if (!_result) - ignerr << "Error sending video record stop request" << std::endl; - }; + std::function cb = + [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending video record stop request" << std::endl; + }; - ignition::msgs::VideoRecord req; - req.set_stop(true); - this->dataPtr->node.Request(this->dataPtr->service, req, cb); + ignition::msgs::VideoRecord req; + req.set_stop(true); + this->dataPtr->node.Request(this->dataPtr->service, req, cb); + } } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/video_recorder/VideoRecorder.hh b/src/gui/plugins/video_recorder/VideoRecorder.hh index b9f98e8ddfc..a551dab782c 100644 --- a/src/gui/plugins/video_recorder/VideoRecorder.hh +++ b/src/gui/plugins/video_recorder/VideoRecorder.hh @@ -20,7 +20,7 @@ #include -#include +#include namespace ignition { @@ -28,8 +28,8 @@ namespace gazebo { class VideoRecorderPrivate; - /// \brief Provides buttons for starting and stopping video recording - class VideoRecorder : public ignition::gui::Plugin + /// \brief Provides video recording cababilities to the 3D scene. + class VideoRecorder : public ignition::gazebo::GuiSystem { Q_OBJECT @@ -42,6 +42,13 @@ namespace gazebo // Documentation inherited public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + // Documentation inherited + public: void Update(const UpdateInfo &_info, EntityComponentManager &_ecm) + override; + + // Documentation inherited + public: bool eventFilter(QObject *_obj, QEvent *_event) override; + /// \brief Callback when video record start request is received /// \param[in] _format Video encoding format public slots: void OnStart(const QString &_format); diff --git a/src/gui/plugins/view_angle/CMakeLists.txt b/src/gui/plugins/view_angle/CMakeLists.txt index db5900ca92a..2e13f9ea06f 100644 --- a/src/gui/plugins/view_angle/CMakeLists.txt +++ b/src/gui/plugins/view_angle/CMakeLists.txt @@ -1,6 +1,8 @@ gz_add_gui_plugin(ViewAngle SOURCES ViewAngle.cc QT_HEADERS ViewAngle.hh + PRIVATE_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering ) diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index 9de7fddf4f2..8f3b34f4317 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -23,15 +23,31 @@ #include #include +#include #include +#include +#include +#include #include +#include +#include +#include #include +#include "ignition/gazebo/Entity.hh" +#include "ignition/gazebo/gui/GuiEvents.hh" + namespace ignition::gazebo { class ViewAnglePrivate { + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + + /// \brief Callback when an animation is complete + private: void OnComplete(); + /// \brief Ignition communication node. public: transport::Node node; @@ -41,11 +57,54 @@ namespace ignition::gazebo /// \brief View Angle service name public: std::string viewAngleService; + /// \brief View Control service name + public: std::string viewControlService; + /// \brief Move gui camera to pose service name public: std::string moveToPoseService; /// \brief gui camera pose public: math::Pose3d camPose; + + /// \brief GUI camera near/far clipping distance (QList order is near, far) + public: QList camClipDist{0.0, 0.0}; + + /// \brief Flag indicating if there is a new camera clipping distance + /// coming from qml side + public: bool newCamClipDist = false; + + /// \brief Checks if there is new camera clipping distance from gui camera, + /// used to update qml side + /// \return True if there is a new clipping distance from gui camera + public: bool UpdateQtCamClipDist(); + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Flag for indicating whether we are in view angle mode or not + public: bool viewingAngle = false; + + /// \brief The pose set during a view angle button press that holds + /// the pose the camera should assume relative to the entit(y/ies). + /// The vector (0, 0, 0) indicates to return the camera back to the home + /// pose originally loaded from the sdf. + public: math::Vector3d viewAngleDirection = math::Vector3d::Zero; + + /// \brief Helper object to move user camera + public: rendering::MoveToHelper moveToHelper; + + /// \brief The current selected entities + public: std::vector selectedEntities; + + /// \brief Last move to animation time + public: std::chrono::time_point prevMoveToTime; + + /// \brief The pose set from the move to pose service. + public: std::optional moveToPoseValue; + + /// \brief Enable legacy features for plugin to work with GzScene3D. + /// Disable them to work with the new MinimalScene plugin. + public: bool legacy{false}; }; } @@ -54,7 +113,7 @@ using namespace gazebo; ///////////////////////////////////////////////// ViewAngle::ViewAngle() - : gui::Plugin(), dataPtr(std::make_unique()) + : ignition::gui::Plugin(), dataPtr(std::make_unique()) { } @@ -62,14 +121,25 @@ ViewAngle::ViewAngle() ViewAngle::~ViewAngle() = default; ///////////////////////////////////////////////// -void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) +void ViewAngle::LoadConfig(const tinyxml2::XMLElement *_pluginElem) { if (this->title.empty()) this->title = "View Angle"; + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("legacy")) + { + elem->QueryBoolText(&this->dataPtr->legacy); + } + } + // For view angle requests this->dataPtr->viewAngleService = "/gui/view_angle"; + // view control requests + this->dataPtr->viewControlService = "/gui/camera/view_control"; + // Subscribe to camera pose std::string topic = "/gui/camera/pose"; this->dataPtr->node.Subscribe( @@ -77,24 +147,104 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) // Move to pose service this->dataPtr->moveToPoseService = "/gui/move_to/pose"; + + ignition::gui::App()->findChild< + ignition::gui::MainWindow *>()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool ViewAngle::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + + // updates qml cam clip distance spin boxes if changed + if (this->dataPtr->UpdateQtCamClipDist()) + { + this->CamClipDistChanged(); + } + } + else if (_event->type() == + ignition::gazebo::gui::events::EntitiesSelected::kType) + { + auto selectedEvent = + reinterpret_cast( + _event); + + if (selectedEvent && !selectedEvent->Data().empty()) + { + for (const auto &_entity : selectedEvent->Data()) + { + if (std::find(this->dataPtr->selectedEntities.begin(), + this->dataPtr->selectedEntities.end(), + _entity) != this->dataPtr->selectedEntities.end()) + continue; + this->dataPtr->selectedEntities.push_back(_entity); + } + } + } + else if (_event->type() == + ignition::gazebo::gui::events::DeselectAllEntities::kType) + { + this->dataPtr->selectedEntities.clear(); + } + + // Standard event processing + return QObject::eventFilter(_obj, _event); } ///////////////////////////////////////////////// void ViewAngle::OnAngleMode(int _x, int _y, int _z) { - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + // Legacy mode: request view angle from GzScene3d + if (this->dataPtr->legacy) + { + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error setting view angle mode" << std::endl; + }; + + msgs::Vector3d req; + req.set_x(_x); + req.set_y(_y); + req.set_z(_z); + + this->dataPtr->node.Request(this->dataPtr->viewAngleService, req, cb); + } + // New behaviour: handle camera movement here + else + { + this->dataPtr->viewingAngle = true; + this->dataPtr->viewAngleDirection = math::Vector3d(_x, _y, _z); + } +} + +///////////////////////////////////////////////// +void ViewAngle::OnViewControl(const QString &_controller) +{ + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) - ignerr << "Error setting view angle mode" << std::endl; + ignerr << "Error setting view controller" << std::endl; }; - ignition::msgs::Vector3d req; - req.set_x(_x); - req.set_y(_y); - req.set_z(_z); + msgs::StringMsg req; + std::string str = _controller.toStdString(); + if (str.find("Orbit") != std::string::npos) + req.set_data("orbit"); + else if (str.find("Ortho") != std::string::npos) + req.set_data("ortho"); + else + { + ignerr << "Unknown view controller selected: " << str << std::endl; + return; + } - this->dataPtr->node.Request(this->dataPtr->viewAngleService, req, cb); + this->dataPtr->node.Request(this->dataPtr->viewControlService, req, cb); } ///////////////////////////////////////////////// @@ -116,17 +266,26 @@ void ViewAngle::SetCamPose(double _x, double _y, double _z, { this->dataPtr->camPose.Set(_x, _y, _z, _roll, _pitch, _yaw); - std::function cb = - [](const ignition::msgs::Boolean &/*_rep*/, const bool _result) + // Legacy mode: request view angle from GzScene3d + if (this->dataPtr->legacy) { - if (!_result) - ignerr << "Error sending move camera to pose request" << std::endl; - }; + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + ignerr << "Error sending move camera to pose request" << std::endl; + }; - ignition::msgs::GUICamera req; - msgs::Set(req.mutable_pose(), this->dataPtr->camPose); + msgs::GUICamera req; + msgs::Set(req.mutable_pose(), this->dataPtr->camPose); - this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); + this->dataPtr->node.Request(this->dataPtr->moveToPoseService, req, cb); + } + // New behaviour: handle camera movement here + else + { + this->dataPtr->moveToPoseValue = {_x, _y, _z, _roll, _pitch, _yaw}; + } } ///////////////////////////////////////////////// @@ -142,6 +301,167 @@ void ViewAngle::CamPoseCb(const msgs::Pose &_msg) } } +///////////////////////////////////////////////// +QList ViewAngle::CamClipDist() const +{ + return this->dataPtr->camClipDist; +} + +///////////////////////////////////////////////// +void ViewAngle::SetCamClipDist(double _near, double _far) +{ + this->dataPtr->camClipDist[0] = _near; + this->dataPtr->camClipDist[1] = _far; + this->dataPtr->newCamClipDist = true; +} + +///////////////////////////////////////////////// +void ViewAnglePrivate::OnRender() +{ + if (!this->camera) + { + auto scene = rendering::sceneFromFirstRenderEngine(); + if (!scene) + return; + + for (unsigned int i = 0; i < scene->NodeCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + scene->NodeByIndex(i)); + if (cam) + { + bool isUserCamera = false; + try + { + isUserCamera = std::get(cam->UserData("user-camera")); + } + catch (std::bad_variant_access &) + { + continue; + } + if (isUserCamera) + { + this->camera = cam; + this->moveToHelper.SetInitCameraPose(this->camera->WorldPose()); + igndbg << "ViewAngle plugin is moving camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + } + + if (!this->camera) + { + ignerr << "ViewAngle camera is not available" << std::endl; + return; + } + } + + // View angle + if (this->viewingAngle) + { + if (this->moveToHelper.Idle()) + { + // Look at the origin if no entities are selected + math::Vector3d lookAt = math::Vector3d::Zero; + if (!this->selectedEntities.empty()) + { + for (const auto &entity : this->selectedEntities) + { + for (auto i = 0u; i < this->camera->Scene()->VisualCount(); + ++i) + { + auto vis = this->camera->Scene()->VisualByIndex(i); + if (!vis) + continue; + + try + { + if (std::get(vis->UserData("gazebo-entity")) != + static_cast(entity)) + { + continue; + } + } + catch (std::bad_variant_access &) + { + continue; + } + + auto pos = vis->WorldPose().Pos(); + lookAt += pos; + } + } + lookAt /= this->selectedEntities.size(); + } + + this->moveToHelper.LookDirection(this->camera, + this->viewAngleDirection, lookAt, + 0.5, std::bind(&ViewAnglePrivate::OnComplete, this)); + this->prevMoveToTime = std::chrono::system_clock::now(); + } + else + { + auto now = std::chrono::system_clock::now(); + std::chrono::duration dt = now - this->prevMoveToTime; + this->moveToHelper.AddTime(dt.count()); + this->prevMoveToTime = now; + } + } + + // Move to pose + if (this->moveToPoseValue) + { + if (this->moveToHelper.Idle()) + { + this->moveToHelper.MoveTo(this->camera, + *(this->moveToPoseValue), + 0.5, std::bind(&ViewAnglePrivate::OnComplete, this)); + this->prevMoveToTime = std::chrono::system_clock::now(); + } + else + { + auto now = std::chrono::system_clock::now(); + std::chrono::duration dt = now - this->prevMoveToTime; + this->moveToHelper.AddTime(dt.count()); + this->prevMoveToTime = now; + } + } + + // Camera clipping plane distance + if (this->newCamClipDist) + { + this->camera->SetNearClipPlane(this->camClipDist[0]); + this->camera->SetFarClipPlane(this->camClipDist[1]); + this->newCamClipDist = false; + } +} + +///////////////////////////////////////////////// +void ViewAnglePrivate::OnComplete() +{ + this->viewingAngle = false; + this->moveToPoseValue.reset(); +} + +///////////////////////////////////////////////// +bool ViewAnglePrivate::UpdateQtCamClipDist() +{ + bool updated = false; + if (std::abs(this->camera->NearClipPlane() - this->camClipDist[0]) > 0.0001) + { + this->camClipDist[0] = this->camera->NearClipPlane(); + updated = true; + } + + if (std::abs(this->camera->FarClipPlane() - this->camClipDist[1]) > 0.0001) + { + this->camClipDist[1] = this->camera->FarClipPlane(); + updated = true; + } + return updated; +} + // Register this plugin IGNITION_ADD_PLUGIN(ignition::gazebo::ViewAngle, ignition::gui::Plugin) diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index d58b064aedd..155d16ae2e4 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -34,6 +34,8 @@ namespace gazebo /// /// ## Configuration /// \ : Set the service to receive view angle requests. + /// \ : Set to true to use with GzScene3D, false to use with + /// MinimalScene. Defaults to true. class ViewAngle : public ignition::gui::Plugin { Q_OBJECT @@ -45,6 +47,13 @@ namespace gazebo NOTIFY CamPoseChanged ) + /// \brief gui camera near/far clipping distance (QList order is near, far) + Q_PROPERTY( + QList camClipDist + READ CamClipDist + NOTIFY CamClipDistChanged + ) + /// \brief Constructor public: ViewAngle(); @@ -54,6 +63,9 @@ namespace gazebo // Documentation inherited public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + // Documentation inherited + private: bool eventFilter(QObject *_obj, QEvent *_event) override; + /// \brief Callback in Qt thread when angle mode changes. /// \param[in] _x The x component of the directional vector for the camera /// to assume. All 0s for x, y, and z indicate the initial camera pose. @@ -63,6 +75,10 @@ namespace gazebo /// to assume. All 0s for x, y, and z indicate the initial camera pose. public slots: void OnAngleMode(int _x, int _y, int _z); + /// \brief Callback in Qt thread when camera view controller changes. + /// \param[in] _mode New camera view controller + public slots: void OnViewControl(const QString &_controller); + /// \brief Get the current gui camera pose. public: Q_INVOKABLE QList CamPose() const; @@ -79,6 +95,17 @@ namespace gazebo /// \param[in] _msg Pose message public: void CamPoseCb(const msgs::Pose &_msg); + /// \brief Get the current gui camera's near and far clipping distances + public: Q_INVOKABLE QList CamClipDist() const; + + /// \brief Notify that the gui camera's near/far clipping distances changed + signals: void CamClipDistChanged(); + + /// \brief Updates gui camera's near/far clipping distances + /// \param[in] _near Near clipping plane distance + /// \param[in] _far Far clipping plane distance + public slots: void SetCamClipDist(double _near, double _far); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 311ca12b3e4..bdf494d73c3 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -22,308 +22,378 @@ import QtQuick.Layouts 1.3 import QtQuick.Controls.Styles 1.4 import "qrc:/qml" -ToolBar { +ColumnLayout { Layout.minimumWidth: 320 Layout.minimumHeight: 380 anchors.fill: parent - background: Rectangle { - color: "transparent" + ToolBar { + Layout.fillWidth: true + background: Rectangle { + color: "transparent" + } + + ButtonGroup { + id: group + } + + GridLayout { + id: views + anchors.horizontalCenter: parent.horizontalCenter + columns: 4 + ToolButton { + id: top + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the top" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 0 + Layout.column: 1 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_top.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(0, 0, -1) + } + } + ToolButton { + id: home + checkable: true + ButtonGroup.group: group + ToolTip.text: "Reset View Angle" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 0 + Layout.column: 3 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_home.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(0, 0, 0) + } + } + ToolButton { + id: left + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the left" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 1 + Layout.column: 0 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_left.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(0, 1, 0) + } + } + ToolButton { + id: front + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the front" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 1 + Layout.column: 1 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_front.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(-1, 0, 0) + } + } + ToolButton { + id: right + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the right" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 1 + Layout.column: 2 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_right.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(0, -1, 0) + } + } + ToolButton { + id: back + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the back" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 1 + Layout.column: 3 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_back.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(1, 0, 0) + } + } + ToolButton { + id: bottom + checkable: true + ButtonGroup.group: group + ToolTip.text: "View from the bottom" + ToolTip.visible: hovered + ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Layout.row: 2 + Layout.column: 1 + contentItem: Image { + fillMode: Image.Pad + horizontalAlignment: Image.AlignHCenter + verticalAlignment: Image.AlignVCenter + source: "view_angle_bottom.png" + sourceSize.width: 24; + sourceSize.height: 24; + } + onClicked: { + ViewAngle.OnAngleMode(0, 0, 1) + } + } + } } - ButtonGroup { - id: group + // Projection + ComboBox { + currentIndex: 0 + model: ListModel { + id: controller + ListElement {text: "Orbit View Control"} + ListElement {text: "Orthographic View Control"} + } + Layout.fillWidth: true + Layout.minimumWidth: 280 + Layout.margins: 10 + onCurrentIndexChanged: { + ViewAngle.OnViewControl(controller.get(currentIndex).text) + } + } + + // Set camera pose + Text { + text: "Camera Pose" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + font.bold: true } GridLayout { - id: views - anchors.horizontalCenter: parent.horizontalCenter - columns: 8 - ToolButton { - id: top - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the top" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + width: parent.width + columns: 6 + + Text { + text: "X (m)" + color: "dimgrey" Layout.row: 0 - Layout.column: 1 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_top.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(0, 0, -1) - } + Layout.column: 0 + leftPadding: 5 } - ToolButton { - id: home - checkable: true - ButtonGroup.group: group - ToolTip.text: "Reset View Angle" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + IgnSpinBox { + id: x + Layout.fillWidth: true Layout.row: 0 - Layout.column: 3 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_home.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(0, 0, 0) - } + Layout.column: 1 + value: ViewAngle.camPose[0] + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) } - ToolButton { - id: left - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the left" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Text { + text: "Y (m)" + color: "dimgrey" Layout.row: 1 Layout.column: 0 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_left.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(0, 1, 0) - } + leftPadding: 5 } - ToolButton { - id: front - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the front" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + IgnSpinBox { + id: y + Layout.fillWidth: true Layout.row: 1 Layout.column: 1 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_front.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(-1, 0, 0) - } + value: ViewAngle.camPose[1] + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Z (m)" + color: "dimgrey" + Layout.row: 2 + Layout.column: 0 + leftPadding: 5 + } + IgnSpinBox { + id: z + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 1 + value: ViewAngle.camPose[2] + maximumValue: Number.MAX_VALUE + minimumValue: -Number.MAX_VALUE + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) } - ToolButton { - id: right - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the right" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + + Text { + text: "Roll (rad)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: roll + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 3 + value: ViewAngle.camPose[3] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) + } + Text { + text: "Pitch (rad)" + color: "dimgrey" Layout.row: 1 Layout.column: 2 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_right.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(0, -1, 0) - } + leftPadding: 5 } - ToolButton { - id: back - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the back" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + IgnSpinBox { + id: pitch + Layout.fillWidth: true Layout.row: 1 Layout.column: 3 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_back.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(1, 0, 0) - } + value: ViewAngle.camPose[4] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) } - ToolButton { - id: bottom - checkable: true - ButtonGroup.group: group - ToolTip.text: "View from the bottom" - ToolTip.visible: hovered - ToolTip.delay: Qt.styleHints.mousePressAndHoldInterval + Text { + text: "Yaw (rad)" + color: "dimgrey" Layout.row: 2 - Layout.column: 1 - contentItem: Image { - fillMode: Image.Pad - horizontalAlignment: Image.AlignHCenter - verticalAlignment: Image.AlignVCenter - source: "view_angle_bottom.png" - sourceSize.width: 24; - sourceSize.height: 24; - } - onClicked: { - ViewAngle.OnAngleMode(0, 0, 1) - } + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: yaw + Layout.fillWidth: true + Layout.row: 2 + Layout.column: 3 + value: ViewAngle.camPose[5] + maximumValue: 6.28 + minimumValue: -6.28 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) } } - // set camera pose - Rectangle { - y: views.height + 10 - width: parent.width - color: "transparent" - - ColumnLayout { - width: parent.width - Text { - text: "Camera Pose" - color: Material.Grey - Layout.row: 4 - Layout.column: 1 - leftPadding: 5 - } - - GridLayout { - width: parent.width - columns: 6 + // Set camera's near/far clipping distance + Text { + text: "Camera's clipping plane distance" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + topPadding: 10 + font.bold: true + } - Text { - text: "X (m)" - color: "dimgrey" - Layout.row: 4 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: x - Layout.fillWidth: true - Layout.row: 4 - Layout.column: 2 - value: ViewAngle.camPose[0] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Y (m)" - color: "dimgrey" - Layout.row: 5 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: y - Layout.fillWidth: true - Layout.row: 5 - Layout.column: 2 - value: ViewAngle.camPose[1] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Z (m)" - color: "dimgrey" - Layout.row: 6 - Layout.column: 1 - leftPadding: 5 - } - IgnSpinBox { - id: z - Layout.fillWidth: true - Layout.row: 6 - Layout.column: 2 - value: ViewAngle.camPose[2] - maximumValue: 1000000 - minimumValue: -1000000 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } + GridLayout { + width: parent.width + columns: 4 - Text { - text: "Roll (rad)" - color: "dimgrey" - Layout.row: 4 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: roll - Layout.fillWidth: true - Layout.row: 4 - Layout.column: 4 - value: ViewAngle.camPose[3] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Pitch (rad)" - color: "dimgrey" - Layout.row: 5 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: pitch - Layout.fillWidth: true - Layout.row: 5 - Layout.column: 4 - value: ViewAngle.camPose[4] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - Text { - text: "Yaw (rad)" - color: "dimgrey" - Layout.row: 6 - Layout.column: 3 - leftPadding: 5 - } - IgnSpinBox { - id: yaw - Layout.fillWidth: true - Layout.row: 6 - Layout.column: 4 - value: ViewAngle.camPose[5] - maximumValue: 6.28 - minimumValue: -6.28 - decimals: 6 - stepSize: 0.01 - onEditingFinished: ViewAngle.SetCamPose(x.value, y.value, z.value, roll.value, pitch.value, yaw.value) - } - } + Text { + text: "Near (m)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 } + IgnSpinBox { + id: nearClip + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: ViewAngle.camClipDist[0] + maximumValue: farClip.value + minimumValue: 0.000001 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamClipDist(nearClip.value, farClip.value) + } + Text { + text: "Far (m)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 2 + leftPadding: 5 + } + IgnSpinBox { + id: farClip + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 3 + value: ViewAngle.camClipDist[1] + maximumValue: Number.MAX_VALUE + minimumValue: nearClip.value + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetCamClipDist(nearClip.value, farClip.value) + } + } + + // Bottom spacer + Item { + width: 10 + Layout.fillHeight: true } } diff --git a/src/gui/plugins/visualization_capabilities/CMakeLists.txt b/src/gui/plugins/visualization_capabilities/CMakeLists.txt new file mode 100644 index 00000000000..9aef2c05979 --- /dev/null +++ b/src/gui/plugins/visualization_capabilities/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_gui_plugin(VisualizationCapabilities + SOURCES + VisualizationCapabilities.cc + QT_HEADERS + VisualizationCapabilities.hh + PUBLIC_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc new file mode 100644 index 00000000000..acc3f66b5fc --- /dev/null +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -0,0 +1,2548 @@ +/* + * 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 "VisualizationCapabilities.hh" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "ignition/rendering/Capsule.hh" +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Geometry.hh" +#include "ignition/gazebo/components/Inertial.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointType.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/ParentLinkName.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/Transparency.hh" +#include "ignition/gazebo/components/Visibility.hh" +#include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/components/World.hh" + +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/rendering/RenderUtil.hh" +#include "ignition/gazebo/rendering/SceneManager.hh" + +namespace ignition::gazebo +{ + class VisualizationCapabilitiesPrivate + { + /// \brief Update the 3D scene. + public: void OnRender(); + + /// \brief Helper function to get all child links of a model entity. + /// \param[in] _entity Entity to find child links + /// \return Vector of child links found for the parent entity + public: std::vector + FindChildLinks(const ignition::gazebo::Entity &_entity); + + /// \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 Finds the child links for given entity from the ECM + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to find child links + /// \return A vector of child links found for the entity + public: std::vector FindChildLinksFromECM( + const ignition::gazebo::EntityComponentManager &_ecm, + const ignition::gazebo::Entity &_entity); + + /// \brief Finds the links (visual parent) that are used to toggle wireframe + /// and transparent view for visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void PopulateViewModeVisualLinks( + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Finds the links (inertial parent) that are used to create child + /// inertia and center of mass visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindInertialLinks(const EntityComponentManager &_ecm); + + /// \brief Retrieve visual + /// \param[in] _id Unique visual (entity) id + /// \return Pointer to requested visual + public: ignition::rendering::VisualPtr VisualById(unsigned int _id); + + public: rendering::VisualPtr CreateVisual( + ignition::gazebo::Entity _id, + const sdf::Visual &_visual, + rendering::VisualPtr &_parent); + + public: rendering::GeometryPtr LoadGeometry( + const sdf::Geometry &_geom, math::Vector3d &_scale, + math::Pose3d &_localPose); + + public: rendering::MaterialPtr LoadMaterial( + const sdf::Material &_material); + + ///////////////////////////////////////////////// + // Transparent + ///////////////////////////////////////////////// + /// \brief View an entity as transparent + /// \param[in] _entity Entity to view as transparent + public: void ViewTransparent(const ignition::gazebo::Entity &_entity); + + /// \brief Callback for view as transparent request + /// \param[in] _msg Request message to set the target to view as + /// transparent + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnViewTransparent(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + ///////////////////////////////////////////////// + // Wireframes + ///////////////////////////////////////////////// + /// \brief View wireframes of specified entity + /// \param[in] _entity Entity to view wireframes + public: void ViewWireframes(const ignition::gazebo::Entity &_entity); + + /// \brief Callback for view wireframes request + /// \param[in] _msg Request message to set the target to view wireframes + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnViewWireframes(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + ///////////////////////////////////////////////// + // Collision + ///////////////////////////////////////////////// + + /// \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 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 + public: bool OnViewCollisions(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + ///////////////////////////////////////////////// + public: rendering::VisualPtr CreateCollision( + ignition::gazebo::Entity _id, + const sdf::Collision &_collision, + ignition::rendering::VisualPtr &_parent); + + ///////////////////////////////////////////////// + // COM + ///////////////////////////////////////////////// + /// \brief View center of mass of specified entity + /// \param[in] _entity Entity to view center of mass + public: void ViewCOM(const Entity &_entity); + + /// \brief Callback for view center of mass request + /// \param[in] _msg Request message to set the target to view center of + /// mass + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnViewCOM(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Create a center of mass visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parent Visual parent + /// \return Visual (center of mass) object created from the inertial + public: ignition::rendering::VisualPtr createCOMVisual( + ignition::gazebo::Entity _id, + const math::Inertiald &_inertia, + ignition::rendering::VisualPtr &_parent); + ///////////////////////////////////////////////// + // Inertia + ///////////////////////////////////////////////// + /// \brief View inertia of specified entity + /// \param[in] _entity Entity to view inertia + public: void ViewInertia(const Entity &_entity); + + /// \brief Callback for view inertia request + /// \param[in] _msg Request message to set the target to view inertia + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnViewInertia(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief Create an inertia visual + /// \param[in] _id Unique visual id + /// \param[in] _inertial Inertial component of the link + /// \param[in] _parent Visual parent + /// \return Visual (center of mass) object created from the inertial + public: ignition::rendering::VisualPtr CreateInertiaVisual( + ignition::gazebo::Entity _id, + const math::Inertiald &_inertia, + ignition::rendering::VisualPtr &_parent); + + ///////////////////////////////////////////////// + // Joints + ///////////////////////////////////////////////// + + /// \brief Callback for view joints request + /// \param[in] _msg Request message to set the target to view center of + /// mass + /// \param[in] _res Response data + /// \return True if the request is received + public: bool OnViewJoints(const msgs::StringMsg &_msg, + msgs::Boolean &_res); + + /// \brief View joints of specified entity + /// \param[in] _entity Entity to view joints + public: void ViewJoints(const Entity &_entity); + + /// \brief Create a joint visual + /// \param[in] _id Unique visual id + /// \param[in] _joint Joint sdf dom + /// \param[in] _childId Joint child id + /// \param[in] _parentId Joint parent id + /// \return Visual (joint) object created from the sdf dom + public: rendering::VisualPtr CreateJointVisual(Entity _id, + const sdf::Joint &_joint, Entity _childId = 0, + Entity _parentId = 0); + + /// \brief Updates the world pose of joint parent visual + /// according to its child. + /// \param[in] _jointId Joint visual id. + public: void UpdateJointParentPose(Entity _jointId); + + ///////////////////////////////////////////////// + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief Keep track of world ID, which is equivalent to the scene's + /// root visual. + /// Defaults to kNullEntity. + public: Entity worldId{kNullEntity}; + + /// \brief Pointer to the rendering scene + public: ignition::rendering::ScenePtr scene{nullptr}; + + /// \brief Scene manager + public: ignition::gazebo::SceneManager sceneManager; + + /// True if the rendering component is initialized + public: bool initialized = false; + + /// \brief A map of model entities and their corresponding children links + public: std::map> modelToLinkEntities; + + /// \brief A map of model entities and their corresponding children models + public: std::map> modelToModelEntities; + + /// \brief New wireframe visuals to be toggled + public: std::vector newTransparentEntities; + + /// \brief A map of link entities and their corresponding children visuals + public: std::map> linkToVisualEntities; + + /// \brief Map of visual entity in Gazebo to visual pointers. + public: std::map visuals; + + ///////////////////////////////////////////////// + // Transparent + ///////////////////////////////////////////////// + /// \brief Target to view as transparent + public: std::string viewTransparentTarget; + + /// \brief A list of links used to toggle transparent mode for visuals + public: std::vector newTransparentVisualLinks; + + /// \brief A map of created transparent visuals and if they are currently + /// visible + public: std::map viewingTransparent; + + /// \brief View transparent service + public: std::string viewTransparentService; + + ///////////////////////////////////////////////// + // Wireframes + ///////////////////////////////////////////////// + + /// \brief Target to view wireframes + public: std::string viewWireframesTarget; + + /// \brief New wireframe visuals to be toggled + public: std::vector newWireframes; + + /// \brief A map of created wireframe visuals and if they are currently + /// visible + public: std::map viewingWireframes; + + /// \brief A list of links used to toggle wireframe mode for visuals + public: std::vector newWireframeVisualLinks; + + /// \brief View transparent service + public: std::string viewWireframesService; + + ///////////////////////////////////////////////// + // COM + ///////////////////////////////////////////////// + + /// \brief New center of mass visuals to be created + public: std::vector newCOMVisuals; + + /// \brief A list of links used to create new center of mass visuals + public: std::vector newCOMLinks; + + /// \brief A map of link entities and if their center of mass visuals + /// are currently visible + public: std::map viewingCOM; + + /// \brief A map of links and their center of mass visuals + public: std::map linkToCOMVisuals; + + /// \brief Target to view center of mass + public: std::string viewCOMTarget; + + /// \brief View center of mass service + public: std::string viewCOMService; + + ///////////////////////////////////////////////// + // Inertia + ///////////////////////////////////////////////// + + /// \brief View inertia service + public: std::string viewInertiaService; + + /// \brief A map of entity ids and their inertials + public: std::map entityInertials; + + /// \brief A map of links and their inertia visuals + public: std::map linkToInertiaVisuals; + + /// \brief A map of link entities and if their inertias are currently + /// visible + public: std::map viewingInertias; + + /// \brief A list of links used to create new inertia visuals + public: std::vector newInertiaLinks; + + /// \brief New inertias to be created + public: std::vector newInertias; + + /// \brief Target to view inertia + public: std::string viewInertiaTarget; + + ///////////////////////////////////////////////// + // Collision + ///////////////////////////////////////////////// + + /// \brief Target to view collisions + public: std::string viewCollisionsTarget; + + /// \brief View collisions service + public: std::string viewCollisionsService; + + /// \brief New collisions to be created + public: std::vector newCollisions; + + /// \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 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; + + ///////////////////////////////////////////////// + // Joints + ///////////////////////////////////////////////// + + /// \brief View joints service name + public: std::string viewJointsService; + + /// \brief Target to view joints + public: std::string viewJointsTarget; + + /// \brief New joint visuals to be created + public: std::vector newJoints; + + /// \brief Finds the models (joint parent) that are used to create + /// joint visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindJointModels(const EntityComponentManager &_ecm); + + /// \brief A list of models used to create new joint visuals + public: std::vector newJointModels; + + /// \brief A map of joint entity ids and their SDF DOM + public: std::map entityJoints; + + /// \brief A map of model entities and their corresponding children links + public: std::map> modelToJointEntities; + + /// \brief A map of created joint entities and if they are currently + /// visible + public: std::map viewingJoints; + + /// \brief A list of joint visuals for which the parent visual poses + /// have to be updated. + public: std::vector updateJointParentPoses; + + /// \brief A map of models entities and link attributes used + /// to create joint visuals + public: std::map> + matchLinksWithEntities; + + }; +} + +using namespace ignition; +using namespace gazebo; + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::OnRender() +{ + if (nullptr == this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (nullptr == this->scene) + { + return; + } + this->sceneManager.SetScene(this->scene); + } + + // create new wireframe visuals + for (const auto &link : this->newWireframeVisualLinks) + { + std::vector visEntities = + this->linkToVisualEntities[link]; + + for (const auto &visEntity : visEntities) + { + if (!this->viewingWireframes[visEntity]) + { + auto wireframeVisual = this->VisualById(visEntity); + if (wireframeVisual) + { + wireframeVisual->SetWireframe(true); + this->viewingWireframes[visEntity] = true; + } + } + } + } + this->newWireframeVisualLinks.clear(); + + // update joint parent visual poses + { + for (const auto &jointEntity : this->updateJointParentPoses) + { + this->UpdateJointParentPose(jointEntity); + } + } + this->updateJointParentPoses.clear(); + + // create new transparent visuals + for (const auto &link : this->newTransparentVisualLinks) + { + std::vector visEntities = + this->linkToVisualEntities[link]; + + for (const auto &visEntity : visEntities) + { + if (!this->viewingTransparent[visEntity]) + { + auto transparencyVisual = this->VisualById(visEntity); + if (transparencyVisual) + { + this->sceneManager.UpdateTransparency(transparencyVisual, + true /* transparent */); + this->viewingTransparent[visEntity] = true; + } + } + } + } + this->newTransparentVisualLinks.clear(); + + // create new inertia visuals + for (const auto &link : this->newInertiaLinks) + { + // create a new id for the inertia visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = i; + if (!this->scene->HasNodeId(id) && !this->scene->HasLightId(id) && + !this->scene->HasSensorId(id) && !this->scene->HasVisualId(id) && + !this->viewingInertias[link]) + { + auto existsVisual = this->VisualById(id); + auto parentInertiaVisual = this->VisualById(link); + + if (existsVisual == nullptr && parentInertiaVisual != nullptr) + { + this->CreateInertiaVisual(id, this->entityInertials[link], + parentInertiaVisual); + } + else + { + continue; + } + this->viewingInertias[link] = true; + this->linkToInertiaVisuals[link] = id; + break; + } + } + } + this->newInertiaLinks.clear(); + + // create new joint visuals + { + for (const auto &model : this->newJointModels) + { + std::vector jointEntities = + this->modelToJointEntities[model]; + + for (const auto &jointEntity : jointEntities) + { + if (!this->scene->HasNodeId(jointEntity) && + !this->scene->HasLightId(jointEntity) && + !this->scene->HasSensorId(jointEntity) && + !this->scene->HasVisualId(jointEntity) && + !this->viewingInertias[jointEntity]) + { + std::string childLinkName = + this->entityJoints[jointEntity].ChildLinkName(); + Entity childId = + this->matchLinksWithEntities[model][childLinkName]; + + std::string parentLinkName = + this->entityJoints[jointEntity].ParentLinkName(); + Entity parentId = + this->matchLinksWithEntities[model][parentLinkName]; + + auto joint = this->entityJoints[jointEntity]; + + auto vis = this->CreateJointVisual( + jointEntity, joint, childId, parentId); + this->viewingJoints[jointEntity] = true; + + // Update joint parent visual pose + if (joint.Axis(1)) + { + this->updateJointParentPoses.push_back(jointEntity); + } + } + } + } + } + this->newJointModels.clear(); + + // create new center of mass visuals + for (const auto &link : this->newCOMLinks) + { + // create a new id for the center of mass visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = i; + if (!this->scene->HasNodeId(id) && !this->scene->HasLightId(id) && + !this->scene->HasSensorId(id) && !this->scene->HasVisualId(id) && + !this->viewingCOM[link]) + { + auto existsVisual = this->VisualById(id); + auto parentInertiaVisual = this->VisualById(link); + + if (existsVisual == nullptr && parentInertiaVisual != nullptr) + { + this->createCOMVisual(id, this->entityInertials[link], + parentInertiaVisual); + } + else + { + continue; + } + this->viewingCOM[link] = true; + this->linkToCOMVisuals[link] = id; + break; + } + } + } + this->newCOMLinks.clear(); + + // create new collision visuals + for (const auto &link : this->newCollisionLinks) + { + std::vector colEntities = + this->linkToCollisionEntities[link]; + + for (const auto &colEntity : colEntities) + { + if (!this->scene->HasNodeId(colEntity) && + !this->scene->HasLightId(colEntity) && + !this->scene->HasSensorId(colEntity) && + !this->scene->HasVisualId(colEntity) && + !this->viewingCollisions[link]) + { + auto parentCollisionVisual = this->VisualById(link); + if (parentCollisionVisual != nullptr) + { + auto vis = this->CreateCollision( + colEntity, + this->entityCollisions[colEntity], + parentCollisionVisual); + + if (vis == nullptr) + { + continue; + } + vis->SetUserData("gui-only", static_cast(true)); + + this->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; + } + } + else + { + continue; + } + } + } + } + this->newCollisionLinks.clear(); + + // View center of mass + { + IGN_PROFILE("IgnRenderer::Render ViewCOM"); + if (!this->viewCOMTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->viewCOMTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewCOM(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewCOMTarget + << "] to view center of mass" << std::endl; + } + + this->viewCOMTarget.clear(); + } + } + + // View inertia + { + IGN_PROFILE("IgnRenderer::Render ViewInertia"); + if (!this->viewInertiaTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->viewInertiaTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewInertia(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewInertiaTarget + << "] to view inertia" << std::endl; + } + + this->viewInertiaTarget.clear(); + } + } + + // view Transparent + { + IGN_PROFILE("IgnRenderer::Render ViewTransparent"); + if (!this->viewTransparentTarget.empty()) + { + rendering::NodePtr targetNode = + this->scene->VisualByName(this->viewTransparentTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewTransparent(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewTransparentTarget + << "] to view as transparent" << std::endl; + } + + this->viewTransparentTarget.clear(); + } + } + + // View collisions + { + IGN_PROFILE("IgnRenderer::Render ViewCollisions"); + if (!this->viewCollisionsTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->viewCollisionsTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewCollisions(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewCollisionsTarget + << "] to view collisions" << std::endl; + } + + this->viewCollisionsTarget.clear(); + } + } + + // View joints + { + IGN_PROFILE("IgnRenderer::Render ViewJoints"); + if (!this->viewJointsTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->viewJointsTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewJoints(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewJointsTarget + << "] to view joints" << std::endl; + } + + this->viewJointsTarget.clear(); + } + } + + // View wireframes + { + IGN_PROFILE("IgnRenderer::Render ViewWireframes"); + if (!this->viewWireframesTarget.empty()) + { + rendering::NodePtr targetNode = + scene->NodeByName(this->viewWireframesTarget); + auto targetVis = std::dynamic_pointer_cast(targetNode); + + if (targetVis && targetVis->HasUserData("gazebo-entity")) + { + Entity targetEntity = + std::get(targetVis->UserData("gazebo-entity")); + this->ViewWireframes(targetEntity); + } + else + { + ignerr << "Unable to find node name [" + << this->viewWireframesTarget + << "] to view wireframes" << std::endl; + } + + this->viewWireframesTarget.clear(); + } + } +} + +///////////////////////////////////////////////// +rendering::VisualPtr VisualizationCapabilitiesPrivate::CreateJointVisual( + Entity _id, const sdf::Joint &_joint, + Entity _childId, Entity _parentId) +{ + if (!this->scene) + { + return rendering::VisualPtr(); + } + + if (this->visuals.find(_id) != this->visuals.end()) + { + return rendering::VisualPtr(); + } + + rendering::VisualPtr parent; + if (_childId != this->worldId) + { + parent = this->VisualById(_childId); + } + + // Name. + std::string name = _joint.Name().empty() ? std::to_string(_id) : + _joint.Name(); + if (parent) + { + name = parent->Name() + "::" + name; + } + + rendering::JointVisualPtr jointVisual = + this->scene->CreateJointVisual(name); + + switch (_joint.Type()) + { + case sdf::JointType::REVOLUTE: + jointVisual->SetType(rendering::JointVisualType::JVT_REVOLUTE); + break; + case sdf::JointType::REVOLUTE2: + jointVisual->SetType(rendering::JointVisualType::JVT_REVOLUTE2); + break; + case sdf::JointType::PRISMATIC: + jointVisual->SetType(rendering::JointVisualType::JVT_PRISMATIC); + break; + case sdf::JointType::UNIVERSAL: + jointVisual->SetType(rendering::JointVisualType::JVT_UNIVERSAL); + break; + case sdf::JointType::BALL: + jointVisual->SetType(rendering::JointVisualType::JVT_BALL); + break; + case sdf::JointType::SCREW: + jointVisual->SetType(rendering::JointVisualType::JVT_SCREW); + break; + case sdf::JointType::GEARBOX: + jointVisual->SetType(rendering::JointVisualType::JVT_GEARBOX); + break; + case sdf::JointType::FIXED: + jointVisual->SetType(rendering::JointVisualType::JVT_FIXED); + break; + default: + jointVisual->SetType(rendering::JointVisualType::JVT_NONE); + break; + } + + if (parent) + { + jointVisual->RemoveParent(); + parent->AddChild(jointVisual); + } + + if (_joint.Axis(1) && + (_joint.Type() == sdf::JointType::REVOLUTE2 || + _joint.Type() == sdf::JointType::UNIVERSAL + )) + { + auto axis1 = _joint.Axis(0)->Xyz(); + auto axis2 = _joint.Axis(1)->Xyz(); + auto axis1UseParentFrame = _joint.Axis(0)->XyzExpressedIn() == "__model__"; + auto axis2UseParentFrame = _joint.Axis(1)->XyzExpressedIn() == "__model__"; + + jointVisual->SetAxis(axis2, axis2UseParentFrame); + + auto it = this->visuals.find(_parentId); + if (it != this->visuals.end()) + { + auto parentName = it->second->Name(); + jointVisual->SetParentAxis( + axis1, parentName, axis1UseParentFrame); + } + } + else if (_joint.Axis(0) && + (_joint.Type() == sdf::JointType::REVOLUTE || + _joint.Type() == sdf::JointType::PRISMATIC + )) + { + auto axis1 = _joint.Axis(0)->Xyz(); + auto axis1UseParentFrame = _joint.Axis(0)->XyzExpressedIn() == "__model__"; + + jointVisual->SetAxis(axis1, axis1UseParentFrame); + } + else + { + // For fixed joint type, scale joint visual to the joint child link + double childSize = + std::max(0.1, parent->BoundingBox().Size().Length()); + auto scale = ignition::math::Vector3d(childSize * 0.2, + childSize * 0.2, childSize * 0.2); + jointVisual->SetLocalScale(scale); + } + + rendering::VisualPtr jointVis = + std::dynamic_pointer_cast(jointVisual); + jointVis->SetUserData("gazebo-entity", static_cast(_id)); + jointVis->SetUserData("pause-update", static_cast(0)); + jointVis->SetUserData("gui-only", static_cast(true)); + jointVis->SetLocalPose(_joint.RawPose()); + this->visuals[_id] = jointVis; + return jointVis; +} + +//////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::UpdateJointParentPose(Entity _jointId) +{ + auto visual = + this->VisualById(_jointId); + + rendering::JointVisualPtr jointVisual = + std::dynamic_pointer_cast(visual); + + auto childPose = jointVisual->WorldPose(); + + if (jointVisual->ParentAxisVisual()) + { + jointVisual->ParentAxisVisual()->SetWorldPose(childPose); + + // scale parent axis visual to the child + auto childScale = jointVisual->LocalScale(); + jointVisual->ParentAxisVisual()->SetLocalScale(childScale); + } +} + +///////////////////////////////////////////////// +rendering::VisualPtr VisualizationCapabilitiesPrivate::CreateInertiaVisual( + ignition::gazebo::Entity _id, + const math::Inertiald &_inertia, + ignition::rendering::VisualPtr &_parent) +{ + std::string name = "Inertia_" + std::to_string(_id); + if (_parent) + name = _parent->Name() + "::" + name; + + rendering::InertiaVisualPtr inertiaVisual = + this->scene->CreateInertiaVisual(name); + inertiaVisual->SetInertial(_inertia); + + rendering::VisualPtr inertiaVis = + std::dynamic_pointer_cast(inertiaVisual); + inertiaVis->SetUserData("gazebo-entity", static_cast(_id)); + inertiaVis->SetUserData("pause-update", static_cast(0)); + inertiaVis->SetUserData("gui-only", static_cast(true)); + this->visuals[_id] = inertiaVis; + if (_parent) + { + inertiaVis->RemoveParent(); + _parent->AddChild(inertiaVis); + } + return inertiaVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr VisualizationCapabilitiesPrivate::CreateCollision( + ignition::gazebo::Entity _id, + const sdf::Collision &_collision, + rendering::VisualPtr &_parent) +{ + sdf::Material material; + material.SetAmbient(math::Color(1.0f, 0.5088f, 0.0468f, 0.7f)); + material.SetDiffuse(math::Color(1.0f, 0.5088f, 0.0468f, 0.7f)); + + 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, _parent); + return collisionVis; +} + +///////////////////////////////////////////////// +rendering::GeometryPtr VisualizationCapabilitiesPrivate::LoadGeometry( + const sdf::Geometry &_geom, math::Vector3d &_scale, + math::Pose3d &_localPose) +{ + if (!this->scene) + return rendering::GeometryPtr(); + + math::Vector3d scale = math::Vector3d::One; + math::Pose3d localPose = math::Pose3d::Zero; + rendering::GeometryPtr geom{nullptr}; + if (_geom.Type() == sdf::GeometryType::BOX) + { + geom = this->scene->CreateBox(); + scale = _geom.BoxShape()->Size(); + } + else if (_geom.Type() == sdf::GeometryType::CAPSULE) + { + auto capsule = this->scene->CreateCapsule(); + capsule->SetRadius(_geom.CapsuleShape()->Radius()); + capsule->SetLength(_geom.CapsuleShape()->Length()); + geom = capsule; + } + else if (_geom.Type() == sdf::GeometryType::CYLINDER) + { + geom = this->scene->CreateCylinder(); + scale.X() = _geom.CylinderShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = _geom.CylinderShape()->Length(); + } + else if (_geom.Type() == sdf::GeometryType::ELLIPSOID) + { + geom = this->scene->CreateSphere(); + scale.X() = _geom.EllipsoidShape()->Radii().X() * 2; + scale.Y() = _geom.EllipsoidShape()->Radii().Y() * 2; + scale.Z() = _geom.EllipsoidShape()->Radii().Z() * 2; + } + else if (_geom.Type() == sdf::GeometryType::PLANE) + { + geom = this->scene->CreatePlane(); + if (geom == nullptr) + return rendering::GeometryPtr(); + scale.X() = _geom.PlaneShape()->Size().X(); + scale.Y() = _geom.PlaneShape()->Size().Y(); + + // Create a rotation for the plane mesh to account for the normal vector. + // 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.PlaneShape()->Normal(); + localPose.Rot().From2Axes(math::Vector3d::UnitZ, normal.Normalized()); + } + else if (_geom.Type() == sdf::GeometryType::SPHERE) + { + geom = this->scene->CreateSphere(); + scale.X() = _geom.SphereShape()->Radius() * 2; + scale.Y() = scale.X(); + scale.Z() = scale.X(); + } + else if (_geom.Type() == sdf::GeometryType::MESH) + { + auto fullPath = asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + if (fullPath.empty()) + { + ignerr << "Mesh geometry missing uri" << std::endl; + return geom; + } + rendering::MeshDescriptor descriptor; + + // Assume absolute path to mesh file + descriptor.meshName = fullPath; + descriptor.subMeshName = _geom.MeshShape()->Submesh(); + descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); + + ignition::common::MeshManager *meshManager = + ignition::common::MeshManager::Instance(); + descriptor.mesh = meshManager->Load(descriptor.meshName); + geom = this->scene->CreateMesh(descriptor); + scale = _geom.MeshShape()->Scale(); + } + else if (_geom.Type() == sdf::GeometryType::HEIGHTMAP) + { + auto fullPath = asFullPath(_geom.HeightmapShape()->Uri(), + _geom.HeightmapShape()->FilePath()); + if (fullPath.empty()) + { + ignerr << "Heightmap geometry missing URI" << std::endl; + return geom; + } + + auto data = std::make_shared(); + if (data->Load(fullPath) < 0) + { + ignerr << "Failed to load heightmap image data from [" << fullPath << "]" + << std::endl; + return geom; + } + + rendering::HeightmapDescriptor descriptor; + descriptor.SetData(data); + descriptor.SetSize(_geom.HeightmapShape()->Size()); + descriptor.SetSampling(_geom.HeightmapShape()->Sampling()); + + for (uint64_t i = 0; i < _geom.HeightmapShape()->TextureCount(); ++i) + { + auto textureSdf = _geom.HeightmapShape()->TextureByIndex(i); + rendering::HeightmapTexture textureDesc; + textureDesc.SetSize(textureSdf->Size()); + textureDesc.SetDiffuse(asFullPath(textureSdf->Diffuse(), + _geom.HeightmapShape()->FilePath())); + textureDesc.SetNormal(asFullPath(textureSdf->Normal(), + _geom.HeightmapShape()->FilePath())); + descriptor.AddTexture(textureDesc); + } + + for (uint64_t i = 0; i < _geom.HeightmapShape()->BlendCount(); ++i) + { + auto blendSdf = _geom.HeightmapShape()->BlendByIndex(i); + rendering::HeightmapBlend blendDesc; + blendDesc.SetMinHeight(blendSdf->MinHeight()); + blendDesc.SetFadeDistance(blendSdf->FadeDistance()); + descriptor.AddBlend(blendDesc); + } + + geom = this->scene->CreateHeightmap(descriptor); + if (nullptr == geom) + { + ignerr << "Failed to create heightmap [" << fullPath << "]" << std::endl; + } + scale = _geom.HeightmapShape()->Size(); + } + else + { + ignerr << "Unsupported geometry type" << std::endl; + } + _scale = scale; + _localPose = localPose; + return geom; +} + +///////////////////////////////////////////////// +rendering::MaterialPtr VisualizationCapabilitiesPrivate::LoadMaterial( + const sdf::Material &_material) +{ + if (!this->scene) + return rendering::MaterialPtr(); + + rendering::MaterialPtr material = this->scene->CreateMaterial(); + material->SetAmbient(_material.Ambient()); + material->SetDiffuse(_material.Diffuse()); + material->SetSpecular(_material.Specular()); + material->SetEmissive(_material.Emissive()); + material->SetRenderOrder(_material.RenderOrder()); + + // parse PBR params + const sdf::Pbr *pbr = _material.PbrMaterial(); + if (pbr) + { + sdf::PbrWorkflow *workflow = nullptr; + const sdf::PbrWorkflow *metal = + pbr->Workflow(sdf::PbrWorkflowType::METAL); + if (metal) + { + double roughness = metal->Roughness(); + double metalness = metal->Metalness(); + material->SetRoughness(roughness); + material->SetMetalness(metalness); + + // roughness map + std::string roughnessMap = metal->RoughnessMap(); + if (!roughnessMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(roughnessMap, _material.FilePath())); + if (!fullPath.empty()) + material->SetRoughnessMap(fullPath); + else + ignerr << "Unable to find file [" << roughnessMap << "]\n"; + } + + // metalness map + std::string metalnessMap = metal->MetalnessMap(); + if (!metalnessMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(metalnessMap, _material.FilePath())); + if (!fullPath.empty()) + material->SetMetalnessMap(fullPath); + else + ignerr << "Unable to find file [" << metalnessMap << "]\n"; + } + workflow = const_cast(metal); + } + else + { + ignerr << "PBR material: currently only metal workflow is supported" + << std::endl; + } + + // albedo map + std::string albedoMap = workflow->AlbedoMap(); + if (!albedoMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(albedoMap, _material.FilePath())); + if (!fullPath.empty()) + { + material->SetTexture(fullPath); + // Use alpha channel for transparency + material->SetAlphaFromTexture(true, 0.5, _material.DoubleSided()); + } + else + ignerr << "Unable to find file [" << albedoMap << "]\n"; + } + + // normal map + std::string normalMap = workflow->NormalMap(); + if (!normalMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(normalMap, _material.FilePath())); + if (!fullPath.empty()) + material->SetNormalMap(fullPath); + else + ignerr << "Unable to find file [" << normalMap << "]\n"; + } + + + // environment map + std::string environmentMap = workflow->EnvironmentMap(); + if (!environmentMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(environmentMap, _material.FilePath())); + if (!fullPath.empty()) + material->SetEnvironmentMap(fullPath); + else + ignerr << "Unable to find file [" << environmentMap << "]\n"; + } + + // emissive map + std::string emissiveMap = workflow->EmissiveMap(); + if (!emissiveMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(emissiveMap, _material.FilePath())); + if (!fullPath.empty()) + material->SetEmissiveMap(fullPath); + else + ignerr << "Unable to find file [" << emissiveMap << "]\n"; + } + + // light map + std::string lightMap = workflow->LightMap(); + if (!lightMap.empty()) + { + std::string fullPath = common::findFile( + asFullPath(lightMap, _material.FilePath())); + if (!fullPath.empty()) + { + unsigned int uvSet = workflow->LightMapTexCoordSet(); + material->SetLightMap(fullPath, uvSet); + } + else + { + ignerr << "Unable to find file [" << lightMap << "]\n"; + } + } + } + return material; +} + +///////////////////////////////////////////////// +rendering::VisualPtr VisualizationCapabilitiesPrivate::CreateVisual( + ignition::gazebo::Entity _id, + const sdf::Visual &_visual, + rendering::VisualPtr &_parent) +{ + if (!this->scene) + return rendering::VisualPtr(); + + if (this->visuals.find(_id) != this->visuals.end()) + { + return rendering::VisualPtr(); + } + + if (!_visual.Geom()) + return rendering::VisualPtr(); + + std::string name = _visual.Name().empty() ? std::to_string(_id) : + _visual.Name(); + if (_parent) + name = _parent->Name() + "::" + name; + if (this->scene->HasVisualName(name)) + { + auto vis = this->scene->VisualByName(name); + this->visuals[_id] = vis; + return vis; + } + rendering::VisualPtr visualVis = this->scene->CreateVisual(name); + visualVis->SetUserData("gazebo-entity", static_cast(_id)); + visualVis->SetUserData("pause-update", static_cast(0)); + visualVis->SetLocalPose(_visual.RawPose()); + + math::Vector3d scale = math::Vector3d::One; + math::Pose3d localPose; + rendering::GeometryPtr geom = + this->LoadGeometry(*_visual.Geom(), scale, localPose); + + if (geom) + { + /// 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. + if (localPose != math::Pose3d::Zero) + { + rendering::VisualPtr geomVis = + this->scene->CreateVisual(name + "_geom"); + geomVis->AddGeometry(geom); + geomVis->SetLocalPose(localPose); + visualVis->AddChild(geomVis); + } + else + { + visualVis->AddGeometry(geom); + } + + visualVis->SetLocalScale(scale); + + // set material + rendering::MaterialPtr material{nullptr}; + if (_visual.Geom()->Type() == sdf::GeometryType::HEIGHTMAP) + { + // Heightmap's material is loaded together with it. + } + else if (_visual.Material()) + { + material = this->LoadMaterial(*_visual.Material()); + } + // Don't set a default material for meshes because they + // may have their own + // TODO(anyone) support overriding mesh material + else if (_visual.Geom()->Type() != sdf::GeometryType::MESH) + { + // create default material + material = this->scene->Material("ign-grey"); + if (!material) + { + material = this->scene->CreateMaterial("ign-grey"); + material->SetAmbient(0.3, 0.3, 0.3); + material->SetDiffuse(0.7, 0.7, 0.7); + material->SetSpecular(1.0, 1.0, 1.0); + material->SetRoughness(0.2f); + material->SetMetalness(1.0f); + } + } + else + { + // meshes created by mesh loader may have their own materials + // update/override their properties based on input sdf element values + auto mesh = std::dynamic_pointer_cast(geom); + for (unsigned int i = 0; i < mesh->SubMeshCount(); ++i) + { + auto submesh = mesh->SubMeshByIndex(i); + auto submeshMat = submesh->Material(); + if (submeshMat) + { + double productAlpha = (1.0-_visual.Transparency()) * + (1.0 - submeshMat->Transparency()); + submeshMat->SetTransparency(1 - productAlpha); + submeshMat->SetCastShadows(_visual.CastShadows()); + } + } + } + + if (material) + { + // set transparency + material->SetTransparency(_visual.Transparency()); + + // cast shadows + material->SetCastShadows(_visual.CastShadows()); + + geom->SetMaterial(material); + // todo(anyone) SetMaterial function clones the input material. + // but does not take ownership of it so we need to destroy it here. + // This is not ideal. We should let ign-rendering handle the lifetime + // of this material + this->scene->DestroyMaterial(material); + } + } + else + { + ignerr << "Failed to load geometry for visual: " << _visual.Name() + << std::endl; + } + + // visibility flags + visualVis->SetVisibilityFlags(_visual.VisibilityFlags()); + + this->visuals[_id] = visualVis; + if (_parent) + _parent->AddChild(visualVis); + + return visualVis; + +} + +///////////////////////////////////////////////// +rendering::VisualPtr VisualizationCapabilitiesPrivate::createCOMVisual( + ignition::gazebo::Entity _id, + const math::Inertiald &_inertia, + rendering::VisualPtr &_parent) +{ + std::string name = "COM_" + std::to_string(_id); + if (_parent) + name = _parent->Name() + "::" + name; + + rendering::COMVisualPtr comVisual = + this->scene->CreateCOMVisual(name); + comVisual->SetInertial(_inertia); + + rendering::VisualPtr comVis = + std::dynamic_pointer_cast(comVisual); + comVis->SetUserData("gazebo-entity", static_cast(_id)); + comVis->SetUserData("pause-update", static_cast(0)); + comVis->SetUserData("gui-only", static_cast(true)); + this->visuals[_id] = comVis; + + if (_parent) + { + comVis->RemoveParent(); + _parent->AddChild(comVis); + } + + return comVis; +} + +ignition::rendering::VisualPtr VisualizationCapabilitiesPrivate::VisualById( + unsigned int _id) +{ + for (unsigned int i = 0; i < this->scene->VisualCount(); ++i) + { + auto visual = this->scene->VisualByIndex(i); + + try { + Entity visualEntity = + std::get(visual->UserData("gazebo-entity")); + + if (visualEntity == _id) + { + return visual; + } + } + catch (std::bad_variant_access &) + { + // It's ok to get here + } + } + return nullptr; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewTransparent( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewTransparentTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewWireframes( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewWireframesTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewCOM( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewCOMTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewJoints( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewJointsTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewInertia( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewInertiaTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +bool VisualizationCapabilitiesPrivate::OnViewCollisions( + const msgs::StringMsg &_msg, msgs::Boolean &_res) +{ + this->viewCollisionsTarget = _msg.data(); + + _res.set_data(true); + return true; +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewCollisions(const Entity &_entity) +{ + std::vector colEntities; + + if (this->linkToCollisionEntities.find(_entity) != + this->linkToCollisionEntities.end()) + { + colEntities = this->linkToCollisionEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + colEntities.insert(colEntities.end(), + this->linkToCollisionEntities[link].begin(), + this->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->viewingCollisions.find(colEntity) == + this->viewingCollisions.end()) + { + this->newCollisions.push_back(_entity); + showColInit = showCol = true; + } + } + + // second loop toggles already created collisions + for (const auto &colEntity : colEntities) + { + if (this->viewingCollisions.find(colEntity) == + this->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->viewingCollisions[colEntity]; + showColInit = true; + } + + auto colVisual = this->VisualById(colEntity); + if (colVisual) + { + this->viewingCollisions[colEntity] = showCol; + colVisual->SetVisible(showCol); + } + } +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewInertia(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->entityInertials.find(_entity) != + this->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle inertia visuals + bool showInertia, showInertiaInit = false; + // first loop looks for new inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->viewingInertias.find(inertiaLink) == + this->viewingInertias.end()) + { + this->newInertias.push_back(_entity); + showInertiaInit = showInertia = true; + } + } + + // second loop toggles already created inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->viewingInertias.find(inertiaLink) == + this->viewingInertias.end()) + continue; + + // when viewing multiple inertias (e.g. _entity is a model), + // boolean for view inertias is based on first inrEntity in list + if (!showInertiaInit) + { + showInertia = !this->viewingInertias[inertiaLink]; + showInertiaInit = true; + } + + Entity inertiaVisualId = this->linkToInertiaVisuals[inertiaLink]; + + auto inertiaVisual = this->VisualById(inertiaVisualId); + if (inertiaVisual) + { + this->viewingInertias[inertiaLink] = showInertia; + inertiaVisual->SetVisible(showInertia); + } + } +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewJoints(const Entity &_entity) +{ + std::vector jointEntities; + if (this->modelToJointEntities.find(_entity) != + this->modelToJointEntities.end()) + { + jointEntities.insert(jointEntities.end(), + this->modelToJointEntities[_entity].begin(), + this->modelToJointEntities[_entity].end()); + } + + if (this->modelToModelEntities.find(_entity) != + this->modelToModelEntities.end()) + { + std::stack modelStack; + modelStack.push(_entity); + + std::vector childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + jointEntities.insert(jointEntities.end(), + this->modelToJointEntities[model].begin(), + this->modelToJointEntities[model].end()); + + childModels = this->modelToModelEntities[model]; + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + + // Toggle joints + bool showJoint, showJointInit = false; + + // first loop looks for new joints + for (const auto &jointEntity : jointEntities) + { + if (this->viewingJoints.find(jointEntity) == + this->viewingJoints.end()) + { + this->newJoints.push_back(_entity); + showJointInit = showJoint = true; + } + } + + // second loop toggles joints + for (const auto &jointEntity : jointEntities) + { + if (this->viewingJoints.find(jointEntity) == + this->viewingJoints.end()) + continue; + + // when viewing multiple joints (e.g. _entity is a model), + // boolean for view joints is based on first jointEntity in list + if (!showJointInit) + { + showJoint = !this->viewingJoints[jointEntity]; + showJointInit = true; + } + + rendering::VisualPtr jointVisual = + this->VisualById(jointEntity); + if (jointVisual == nullptr) + { + ignerr << "Could not find visual for entity [" << jointEntity + << "]" << std::endl; + continue; + } + + this->viewingJoints[jointEntity] = showJoint; + jointVisual->SetVisible(showJoint); + } +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewCOM(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->entityInertials.find(_entity) != + this->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle center of mass visuals + bool showCOM, showCOMInit = false; + // first loop looks for new center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->viewingCOM.find(inertiaLink) == + this->viewingCOM.end()) + { + this->newCOMVisuals.push_back(_entity); + showCOMInit = showCOM = true; + } + } + + // second loop toggles already created center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->viewingCOM.find(inertiaLink) == + this->viewingCOM.end()) + continue; + + // when viewing multiple center of mass visuals (e.g. _entity is a model), + // boolean for view center of mass is based on first inertiaEntity in list + if (!showCOMInit) + { + showCOM = !this->viewingCOM[inertiaLink]; + showCOMInit = true; + } + + Entity comVisualId = this->linkToCOMVisuals[inertiaLink]; + + auto comVisual = this->VisualById(comVisualId); + if (comVisual) + { + this->viewingCOM[inertiaLink] = showCOM; + comVisual->SetVisible(showCOM); + } + } +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewWireframes(const Entity &_entity) +{ + std::vector visEntities; + + if (this->linkToVisualEntities.find(_entity) != + this->linkToVisualEntities.end()) + { + visEntities = this->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + visEntities.insert(visEntities.end(), + this->linkToVisualEntities[link].begin(), + this->linkToVisualEntities[link].end()); + } + + // Toggle wireframes + bool showWireframe, showWireframeInit = false; + + // first loop looks for new wireframes + for (const auto &visEntity : visEntities) + { + if (this->viewingWireframes.find(visEntity) == + this->viewingWireframes.end()) + { + this->newWireframes.push_back(_entity); + showWireframeInit = showWireframe = true; + } + } + + // second loop toggles wireframes + for (const auto &visEntity : visEntities) + { + if (this->viewingWireframes.find(visEntity) == + this->viewingWireframes.end()) + continue; + + // when viewing multiple wireframes (e.g. _entity is a model), + // boolean for view wireframe is based on first visEntity in list + if (!showWireframeInit) + { + showWireframe = !this->viewingWireframes[visEntity]; + showWireframeInit = true; + } + + auto wireframesVisual = this->VisualById(visEntity); + if (wireframesVisual) + { + this->viewingWireframes[visEntity] = showWireframe; + wireframesVisual->SetWireframe(showWireframe); + } + } +} + +///////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::ViewTransparent(const Entity &_entity) +{ + std::vector visEntities; + + if (this->linkToVisualEntities.find(_entity) != + this->linkToVisualEntities.end()) + { + visEntities = this->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + visEntities.insert(visEntities.end(), + this->linkToVisualEntities[link].begin(), + this->linkToVisualEntities[link].end()); + } + + // Toggle transparent mode + bool showTransparent, showTransparentInit = false; + + // first loop looks for new transparent entities + for (const auto &visEntity : visEntities) + { + if (this->viewingTransparent.find(visEntity) == + this->viewingTransparent.end()) + { + this->newTransparentEntities.push_back(_entity); + showTransparentInit = showTransparent = true; + } + } + + // second loop toggles transparent mode + for (const auto &visEntity : visEntities) + { + if (this->viewingTransparent.find(visEntity) == + this->viewingTransparent.end()) + continue; + + // when viewing multiple transparent visuals (e.g. _entity is a model), + // boolean for view as transparent is based on first visEntity in list + if (!showTransparentInit) + { + showTransparent = !this->viewingTransparent[visEntity]; + showTransparentInit = true; + } + + auto transparentVisual = this->VisualById(visEntity); + if (transparentVisual) + { + this->viewingTransparent[visEntity] = showTransparent; + + this->sceneManager.UpdateTransparency(transparentVisual, + showTransparent); + } + } +} + +///////////////////////////////////////////////// +std::vector VisualizationCapabilitiesPrivate::FindChildLinks( + const Entity &_entity) +{ + std::vector links; + + if (this->modelToLinkEntities.find(_entity) != + this->modelToLinkEntities.end()) + { + links.insert(links.end(), + this->modelToLinkEntities[_entity].begin(), + this->modelToLinkEntities[_entity].end()); + } + + if (this->modelToModelEntities.find(_entity) != + this->modelToModelEntities.end()) + { + std::stack modelStack; + modelStack.push(_entity); + + std::vector childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + links.insert(links.end(), + this->modelToLinkEntities[model].begin(), + this->modelToLinkEntities[model].end()); + + childModels = this->modelToModelEntities[model]; + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + + return links; +} + +////////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::FindJointModels( + const EntityComponentManager &_ecm) +{ + if (this->newJoints.empty()) + { + return; + } + + for (const auto &entity : this->newJoints) + { + std::vector models; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId})) + { + std::stack modelStack; + modelStack.push(entity); + + std::vector childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + models.push_back(model); + + childModels = + _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Model()); + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + else + { + ignerr << "Entity [" << entity + << "] for viewing joints must be a model" + << std::endl; + continue; + } + + this->newJointModels.insert(this->newJointModels.end(), + models.begin(), + models.end()); + } + this->newJoints.clear(); +} + +////////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::FindInertialLinks( + const EntityComponentManager &_ecm) +{ + for (const auto &entity : this->newInertias) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing inertia must be a model or link" + << std::endl; + continue; + } + + this->newInertiaLinks.insert(this->newInertiaLinks.end(), + links.begin(), + links.end()); + } + this->newInertias.clear(); + + for (const auto &entity : this->newCOMVisuals) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing center of mass must be a model or link" + << std::endl; + continue; + } + + this->newCOMLinks.insert(this->newCOMLinks.end(), + links.begin(), + links.end()); + } + this->newCOMVisuals.clear(); +} + +////////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::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}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, 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(); +} + +////////////////////////////////////////////////// +void VisualizationCapabilitiesPrivate::PopulateViewModeVisualLinks( + const EntityComponentManager &_ecm) +{ + // Find links to toggle wireframes + for (const auto &entity : this->newWireframes) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing wireframe must be a model or link" + << std::endl; + continue; + } + + this->newWireframeVisualLinks.insert(this->newWireframeVisualLinks.end(), + links.begin(), + links.end()); + } + this->newWireframes.clear(); + + // Find links to view as transparent + for (const auto &entity : this->newTransparentEntities) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing as transparent must be a model or link" + << std::endl; + continue; + } + + this->newTransparentVisualLinks.insert( + this->newTransparentVisualLinks.end(), + links.begin(), + links.end()); + } + this->newTransparentEntities.clear(); +} + +////////////////////////////////////////////////// +std::vector VisualizationCapabilitiesPrivate::FindChildLinksFromECM( + const EntityComponentManager &_ecm, const Entity &_entity) +{ + std::vector links; + if (_ecm.EntityMatches(_entity, + std::set{components::Model::typeId})) + { + std::stack modelStack; + modelStack.push(_entity); + + std::vector childLinks, childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Link()); + links.insert(links.end(), + childLinks.begin(), + childLinks.end()); + + childModels = + _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Model()); + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + else if (_ecm.EntityMatches(_entity, + std::set{components::Link::typeId})) + { + links.push_back(_entity); + } + return links; +} + +///////////////////////////////////////////////// +VisualizationCapabilities::VisualizationCapabilities() + : GuiSystem(), + dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +VisualizationCapabilities::~VisualizationCapabilities() = default; + +////////////////////////////////////////////////// +void VisualizationCapabilities::Update(const UpdateInfo &, + EntityComponentManager &_ecm) +{ + if (!this->dataPtr->initialized) + { + _ecm.EachNew( + [&](const Entity & _entity, + const components::World *, + const components::Scene *)->bool + { + this->dataPtr->worldId = _entity; + return true; + }); + + _ecm.Each( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->modelToLinkEntities[_parent->Data()].push_back(_entity); + // used for joints + this->dataPtr->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // inertials + _ecm.Each( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->dataPtr->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + + // joints + _ecm.Each( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) + { + joint.SetAxis(1, jointAxis2->Data()); + } + + this->dataPtr->entityJoints[_entity] = joint; + this->dataPtr->modelToJointEntities[_parentModel->Data()] + .push_back(_entity); + return true; + }); + + // visuals + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CastShadows *, + const components::Transparency *, + const components::VisibilityFlags *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->linkToVisualEntities[_parent->Data()] + .push_back(_entity); + return true; + }); + + _ecm.Each( + [&](const Entity &_entity, + const components::Model *, + const components::Name *, + const components::Pose *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->modelToModelEntities[_parent->Data()].push_back(_entity); + 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->dataPtr->entityCollisions[_entity] = _collElement->Data(); + this->dataPtr->linkToCollisionEntities[_parent->Data()] + .push_back(_entity); + return true; + }); + this->dataPtr->initialized = true; + } + else + { + _ecm.EachNew( + [&](const Entity & _entity, + const components::World *, + const components::Scene *)->bool + { + this->dataPtr->worldId = _entity; + return true; + }); + + _ecm.EachNew( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->modelToLinkEntities[_parent->Data()].push_back(_entity); + // used for joints + this->dataPtr->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // inertials + _ecm.EachNew( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->dataPtr->entityInertials[_entity] = _inrElement->Data(); + return true; + }); + + // visuals + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *, + const components::Pose *, + const components::Geometry *, + const components::CastShadows *, + const components::Transparency *, + const components::VisibilityFlags *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->linkToVisualEntities[_parent->Data()] + .push_back(_entity); + return true; + }); + + // joints + _ecm.EachNew( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) + { + joint.SetAxis(1, jointAxis2->Data()); + } + + this->dataPtr->entityJoints[_entity] = joint; + this->dataPtr->modelToJointEntities[_parentModel->Data()] + .push_back(_entity); + return true; + }); + + _ecm.EachNew( + [&](const Entity &_entity, + const components::Model *, + const components::Name *, + const components::Pose *, + const components::ParentEntity *_parent)->bool + { + this->dataPtr->modelToModelEntities[_parent->Data()].push_back(_entity); + 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->dataPtr->entityCollisions[_entity] = _collElement->Data(); + this->dataPtr->linkToCollisionEntities[_parent->Data()] + .push_back(_entity); + return true; + }); + } + + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Model *)->bool + { + this->dataPtr->modelToLinkEntities.erase(_entity); + this->dataPtr->modelToModelEntities.erase(_entity); + return true; + }); + + // joints + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Joint *)->bool + { + this->dataPtr->entityJoints.erase(_entity); + this->dataPtr->viewingJoints.erase(_entity); + return true; + }); + + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Link *)->bool + { + this->dataPtr->linkToVisualEntities.erase(_entity); + return true; + }); + + this->dataPtr->PopulateViewModeVisualLinks(_ecm); + this->dataPtr->FindInertialLinks(_ecm); + this->dataPtr->FindJointModels(_ecm); + this->dataPtr->FindCollisionLinks(_ecm); +} + +///////////////////////////////////////////////// +void VisualizationCapabilities::LoadConfig(const tinyxml2::XMLElement *) +{ + if (this->title.empty()) + this->title = "VisualizationCapabilities"; + + // view as transparent service + this->dataPtr->viewTransparentService = "/gui/view/transparent"; + this->dataPtr->node.Advertise(this->dataPtr->viewTransparentService, + &VisualizationCapabilitiesPrivate::OnViewTransparent, this->dataPtr.get()); + ignmsg << "View as transparent service on [" + << this->dataPtr->viewTransparentService << "]" << std::endl; + + // view wireframes service + this->dataPtr->viewWireframesService = "/gui/view/wireframes"; + this->dataPtr->node.Advertise(this->dataPtr->viewWireframesService, + &VisualizationCapabilitiesPrivate::OnViewWireframes, this->dataPtr.get()); + ignmsg << "View as wireframes service on [" + << this->dataPtr->viewWireframesService << "]" << std::endl; + + // view center of mass service + this->dataPtr->viewCOMService = "/gui/view/com"; + this->dataPtr->node.Advertise(this->dataPtr->viewCOMService, + &VisualizationCapabilitiesPrivate::OnViewCOM, this->dataPtr.get()); + ignmsg << "View center of mass service on [" + << this->dataPtr->viewCOMService << "]" << std::endl; + + // view inertia service + this->dataPtr->viewInertiaService = "/gui/view/inertia"; + this->dataPtr->node.Advertise(this->dataPtr->viewInertiaService, + &VisualizationCapabilitiesPrivate::OnViewInertia, this->dataPtr.get()); + ignmsg << "View inertia service on [" + << this->dataPtr->viewInertiaService << "]" << std::endl; + + // view collisions service + this->dataPtr->viewCollisionsService = "/gui/view/collisions"; + this->dataPtr->node.Advertise(this->dataPtr->viewCollisionsService, + &VisualizationCapabilitiesPrivate::OnViewCollisions, + this->dataPtr.get()); + ignmsg << "View collisions service on [" + << this->dataPtr->viewCollisionsService << "]" << std::endl; + + // view collisions service + this->dataPtr->viewJointsService = "/gui/view/joints"; + this->dataPtr->node.Advertise(this->dataPtr->viewJointsService, + &VisualizationCapabilitiesPrivate::OnViewJoints, + this->dataPtr.get()); + ignmsg << "View joints service on [" + << this->dataPtr->viewJointsService << "]" << std::endl; + + ignition::gui::App()->findChild + ()->installEventFilter(this); +} + +//////////////////////////////////////////////// +bool VisualizationCapabilities::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == ignition::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + return QObject::eventFilter(_obj, _event); +} + + +// Register this plugin +IGNITION_ADD_PLUGIN(ignition::gazebo::VisualizationCapabilities, + ignition::gui::Plugin) diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.hh b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.hh new file mode 100644 index 00000000000..99a400f7eba --- /dev/null +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.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_GUI_VISUALIZATIONCAPABILITIES_HH_ +#define IGNITION_GAZEBO_GUI_VISUALIZATIONCAPABILITIES_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ + class VisualizationCapabilitiesPrivate; + + /// \brief Allows to visualize transparent, collisions, inertial, + /// CoM and more. + class VisualizationCapabilities : public ignition::gazebo::GuiSystem + { + Q_OBJECT + + /// \brief Constructor + public: VisualizationCapabilities(); + + /// \brief Destructor + public: ~VisualizationCapabilities() override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml new file mode 100644 index 00000000000..873da30014a --- /dev/null +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qml @@ -0,0 +1,28 @@ +/* + * 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.0 +import QtQuick.Controls 2.0 +import QtQuick.Layouts 1.3 + +// TODO: remove invisible rectangle, see +// https://github.com/ignitionrobotics/ign-gui/issues/220 +Rectangle { + visible: false + Layout.minimumWidth: 100 + Layout.minimumHeight: 100 +} diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qrc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qrc new file mode 100644 index 00000000000..e1522da1d52 --- /dev/null +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.qrc @@ -0,0 +1,5 @@ + + + VisualizationCapabilities.qml + + diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index aed80f1a067..7211d9e8ef1 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -364,6 +364,13 @@ void VisualizeLidar::UpdateType(int _type) this->dataPtr->lidar->SetType(this->dataPtr->visualType); } +////////////////////////////////////////////////// +void VisualizeLidar::UpdateSize(int _size) +{ + std::lock_guard lock(this->dataPtr->serviceMutex); + this->dataPtr->lidar->SetSize(_size); +} + ////////////////////////////////////////////////// void VisualizeLidar::OnTopic(const QString &_topicName) { diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index e06dee0bd2e..7eebe699772 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -90,6 +90,10 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE /// \param[in] _type Index of selected visual type public: Q_INVOKABLE void UpdateType(int _type); + /// \brief Set lidar visualization size + /// \param[in] _size Size of lidar visualization + public: Q_INVOKABLE void UpdateSize(int _size); + /// \brief Get the topic list as a string /// \return Message type public: Q_INVOKABLE QStringList TopicList() const; diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml index dbe9d680ee0..fa6fdea0272 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.qml +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.qml @@ -25,7 +25,7 @@ GridLayout { columns: 6 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 300 + Layout.minimumHeight: 400 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -131,4 +131,20 @@ GridLayout { VisualizeLidar.UpdateType(typeCombo.currentIndex); } } + + Text { + Layout.columnSpan: 2 + id: pointSizeText + color: "dimgrey" + text: "Point Size" + } + + IgnSpinBox { + Layout.columnSpan: 2 + id: pointSize + maximumValue: 1000 + minimumValue: 1 + value: 1 + onEditingFinished: VisualizeLidar.UpdateSize(pointSize.value) + } } diff --git a/src/ign.cc b/src/ign.cc index 9faf652e09e..9a384281d81 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -121,7 +121,8 @@ extern "C" int runServer(const char *_sdfString, int _recordResources, int _logOverwrite, int _logCompress, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, - const char *_file, const char *_recordTopics) + const char *_file, const char *_recordTopics, + int _headless) { ignition::gazebo::ServerConfig serverConfig; @@ -334,6 +335,8 @@ extern "C" int runServer(const char *_sdfString, serverConfig.SetPhysicsEngine(_physicsEngine); } + serverConfig.SetHeadlessRendering(_headless); + if (_renderEngineServer != nullptr && std::strlen(_renderEngineServer) > 0) { serverConfig.SetRenderEngineServer(_renderEngineServer); @@ -355,7 +358,7 @@ extern "C" int runServer(const char *_sdfString, } ////////////////////////////////////////////////// -extern "C" int runGui(const char *_guiConfig) +extern "C" int runGui(const char *_guiConfig, const char *_renderEngine) { // argc and argv are going to be passed to a QApplication. The Qt // documentation has a warning about these: @@ -368,5 +371,6 @@ extern "C" int runGui(const char *_guiConfig) // be converted to a const char *. The const cast is here to prevent a warning // since we do need to pass a char* to runGui char *argv = const_cast("ign-gazebo-gui"); - return ignition::gazebo::gui::runGui(argc, &argv, _guiConfig); + return ignition::gazebo::gui::runGui( + argc, &argv, _guiConfig, _renderEngine); } diff --git a/src/ign.hh b/src/ign.hh index 3b7cd3c5e4e..28e5c26b97f 100644 --- a/src/ign.hh +++ b/src/ign.hh @@ -54,6 +54,7 @@ extern "C" const char *worldInstallDir(); /// \param[in] _file Path to file being loaded /// \param[in] _recordTopics Colon separated list of topics to record. Leave /// null to record the default topics. +/// \param[in] _headless True if server rendering should run headless /// \return 0 if successful, 1 if not. extern "C" int runServer(const char *_sdfString, int _iterations, int _run, float _hz, int _levels, @@ -62,12 +63,13 @@ extern "C" int runServer(const char *_sdfString, int _logCompress, const char *_playback, const char *_physicsEngine, const char *_renderEngineServer, const char *_renderEngineGui, const char *_file, - const char *_recordTopics); + const char *_recordTopics, int _headless); /// \brief External hook to run simulation GUI. /// \param[in] _guiConfig Path to Ignition GUI configuration file. +/// \param[in] _renderEngine --render-engine-gui option /// \return 0 if successful, 1 if not. -extern "C" int runGui(const char *_guiConfig); +extern "C" int runGui(const char *_guiConfig, const char *_renderEngine); /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, diff --git a/src/network/NetworkConfig.cc b/src/network/NetworkConfig.cc index c8e68e53695..9a51e5ca30e 100644 --- a/src/network/NetworkConfig.cc +++ b/src/network/NetworkConfig.cc @@ -50,14 +50,14 @@ NetworkConfig NetworkConfig::FromValues(const std::string &_role, else { config.role = NetworkRole::None; - ignwarn << "Invalid setting for IGN_GAZEBO_NETWORK_ROLE: " << role + ignwarn << "Invalid setting for network role: " << role << "(expected: PRIMARY, SECONDARY, READONLY)" << ", distributed sim disabled" << std::endl; } } else { - ignwarn << "IGN_GAZEBO_NETWORK_ROLE not set" + ignwarn << "Network role not set" << ", distributed sim disabled" << std::endl; } @@ -68,8 +68,8 @@ NetworkConfig NetworkConfig::FromValues(const std::string &_role, if (config.numSecondariesExpected == 0) { config.role = NetworkRole::None; - ignwarn << "Detected IGN_GAZEBO_NETWORK_ROLE=PRIMARY, but " - << "IGN_GAZEBO_NETWORK_SECONDARIES not set, " + ignwarn << "Detected network role as PRIMARY, but " + << "network secondaries not set, " << "no distributed sim available" << std::endl; } } diff --git a/src/rendering/CMakeLists.txt b/src/rendering/CMakeLists.txt index d885c6b3c4e..801cd72735b 100644 --- a/src/rendering/CMakeLists.txt +++ b/src/rendering/CMakeLists.txt @@ -28,3 +28,4 @@ target_link_libraries(${rendering_target} install(TARGETS ${rendering_target} DESTINATION ${IGN_LIB_INSTALL_DIR}) +set(rendering_target ${rendering_target} PARENT_SCOPE) diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 5bb2d771160..ff14b22a032 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -53,10 +54,15 @@ #include "ignition/gazebo/components/Actor.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/CastShadows.hh" +#include "ignition/gazebo/components/ChildLinkName.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/Inertial.hh" +#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/LightCmd.hh" @@ -65,10 +71,13 @@ #include "ignition/gazebo/components/Model.hh" #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/Pose.hh" #include "ignition/gazebo/components/RgbdCamera.hh" #include "ignition/gazebo/components/Scene.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Temperature.hh" #include "ignition/gazebo/components/TemperatureRange.hh" @@ -76,6 +85,7 @@ #include "ignition/gazebo/components/Transparency.hh" #include "ignition/gazebo/components/Visibility.hh" #include "ignition/gazebo/components/Visual.hh" +#include "ignition/gazebo/components/VisualCmd.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/EntityComponentManager.hh" @@ -96,9 +106,26 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Create rendering entities /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information public: void CreateRenderingEntities(const EntityComponentManager &_ecm, const UpdateInfo &_info); + /// \brief Create rendering entities during the first update. This + /// is called by CreateRenderingEntities. This calls _ecm.Each + /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information + /// \TODO(anyone) Combine with CreateEntitiesRuntime + public: void CreateEntitiesFirstUpdate(const EntityComponentManager &_ecm, + const UpdateInfo &_info); + + /// \brief Create rendering entities during subsequent updates. This + /// is called by CreateRenderingEntities. This calls _ecm.EachNew + /// \param[in] _ecm The entity-component manager + /// \param[in] _info Update information + /// \TODO(anyone) Combine with CreateEntitiesFirstUpdate + public: void CreateEntitiesRuntime(const EntityComponentManager &_ecm, + const UpdateInfo &_info); + /// \brief Remove rendering entities /// \param[in] _ecm The entity-component manager public: void RemoveRenderingEntities(const EntityComponentManager &_ecm, @@ -108,6 +135,17 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _ecm The entity-component manager public: void UpdateRenderingEntities(const EntityComponentManager &_ecm); + /// \breif Helper function to add new sensors + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Sensor entity + /// \param[in] _sdfData Sensor data + /// \param[in] _parent Parent entity + /// \param[in] _topicSuffix Suffix for sensor topic + public: void AddNewSensor( + const EntityComponentManager &_ecm, Entity _entity, + const sdf::Sensor &_sdfData, Entity _parent, + const std::string &_topicSuffix); + /// \brief Total time elapsed in simulation. This will not increase while /// paused. public: std::chrono::steady_clock::duration simTime{0}; @@ -121,11 +159,15 @@ class ignition::gazebo::RenderUtilPrivate //// \brief True to enable sky in the scene public: bool skyEnabled = false; - /// \brief Scene background color - public: math::Color backgroundColor = math::Color::Black; + /// \brief Scene background color. This is optional because a is + /// always present, which has a default background color value. This + /// backgroundColor variable is used to override the value. + public: std::optional backgroundColor = math::Color::Black; - /// \brief Ambient color - public: math::Color ambientLight = math::Color(1.0, 1.0, 1.0, 1.0); + /// \brief Ambient color. This is optional because an is always + /// present, which has a default ambient light value. This ambientLight + /// variable is used to override the value. + public: std::optional ambientLight; /// \brief Scene manager public: SceneManager sceneManager; @@ -143,40 +185,48 @@ class ignition::gazebo::RenderUtilPrivate /// \brief Flag to indicate if the current GL context should be used public: bool useCurrentGLContext = false; + /// \brief Window ID handle + public: std::string winID = ""; + /// \brief New scenes to be created public: std::vector newScenes; + /// \brief is headless mode active + public: bool isHeadlessRendering = false; + /// \brief New models to be created. The elements in the tuple are: /// [0] entity id, [1], SDF DOM, [2] parent entity id, [3] sim iteration public: std::vector> newModels; /// \brief New links to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id public: std::vector> newLinks; /// \brief New visuals to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id public: std::vector> newVisuals; /// \brief New actors to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id - public: std::vector> newActors; + /// [0] entity id, [1] SDF DOM, [2] actor name, [3] parent entity id + public: std::vector> + newActors; /// \brief New lights to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id - public: std::vector> newLights; + /// [0] entity id, [1] SDF DOM, [2] light name, [3] parent entity id + public: std::vector> + newLights; /// \brief A map of entity light ids and light visuals public: std::map matchLightWithVisuals; /// \brief New sensors to be created. The elements in the tuple are: - /// [0] entity id, [1], SDF DOM, [2] parent entity id + /// [0] entity id, [1] SDF DOM, [2] parent entity id 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 + /// [0] entity id, [1] particle emitter, [2] parent entity id public: std::vector> newParticleEmitters; @@ -202,6 +252,25 @@ class ignition::gazebo::RenderUtilPrivate /// \brief A map of entity ids and light updates. public: std::vector entityLightsCmdToDelete; + /// \brief A map of entity ids and visual updates. + public: std::map entityVisuals; + + /// \brief A vector of entity ids of VisualCmds to delete + public: std::vector entityVisualsCmdToDelete; + + /// \brief Visual material equality comparision function + /// TODO(anyone) Currently only checks for material colors equality, + /// need to extend to others (e.g., PbrMaterial) + public: std::function + materialEql { [](const sdf::Material &_a, const sdf::Material &_b) + { + return + _a.Ambient() == _b.Ambient() && + _a.Diffuse() == _b.Diffuse() && + _a.Specular() == _b.Specular() && + _a.Emissive() == _b.Emissive(); + }}; + /// \brief A map of entity ids and actor transforms. public: std::map> actorTransforms; @@ -220,6 +289,9 @@ class ignition::gazebo::RenderUtilPrivate /// All temperatures are in Kelvin. public: std::map> entityTemp; + /// \brief A map of entity ids and label data for datasets annotations + public: std::unordered_map entityLabel; + /// \brief A map of entity ids and wire boxes public: std::unordered_map wireBoxes; @@ -301,6 +373,105 @@ class ignition::gazebo::RenderUtilPrivate /// \param[in] _node Node to be restored. public: void LowlightNode(const rendering::NodePtr &_node); + /// \brief New joint visuals to be created + public: std::vector newJoints; + + /// \brief Finds the models (joint parent) that are used to create + /// joint visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindJointModels(const EntityComponentManager &_ecm); + + /// \brief A list of models used to create new joint visuals + public: std::vector newJointModels; + + /// \brief A map of joint entity ids and their SDF DOM + public: std::map entityJoints; + + /// \brief A map of model entities and their corresponding children links + public: std::map> modelToJointEntities; + + /// \brief A map of created joint entities and if they are currently + /// visible + public: std::map viewingJoints; + + /// \brief A list of joint visuals for which the parent visual poses + /// have to be updated. + public: std::vector updateJointParentPoses; + + /// \brief A map of models entities and link attributes used + /// to create joint visuals + public: std::map> + matchLinksWithEntities; + + /// \brief New center of mass visuals to be created + public: std::vector newCOMVisuals; + + /// \brief A list of links used to create new center of mass visuals + public: std::vector newCOMLinks; + + /// \brief A map of link entities and if their center of mass visuals + /// are currently visible + public: std::map viewingCOM; + + /// \brief New inertias to be created + public: std::vector newInertias; + + /// \brief A map of links and their center of mass visuals + public: std::map linkToCOMVisuals; + + /// \brief Finds the child links for given entity from the ECM + /// \param[in] _ecm The entity-component manager + /// \param[in] _entity Entity to find child links + /// \return A vector of child links found for the entity + public: std::vector FindChildLinksFromECM( + const EntityComponentManager &_ecm, const Entity &_entity); + + /// \brief Finds the links (inertial parent) that are used to create child + /// inertia and center of mass visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void FindInertialLinks(const EntityComponentManager &_ecm); + + /// \brief A list of links used to create new inertia visuals + public: std::vector newInertiaLinks; + + /// \brief A map of entity ids and their inertials + public: std::map entityInertials; + + /// \brief A map of link entities and if their inertias are currently + /// visible + public: std::map viewingInertias; + + /// \brief A map of links and their inertia visuals + public: std::map linkToInertiaVisuals; + + /// \brief New wireframe visuals to be toggled + public: std::vector newWireframes; + + /// \brief New wireframe visuals to be toggled + public: std::vector newTransparentEntities; + + /// \brief Finds the links (visual parent) that are used to toggle wireframe + /// and transparent view for visuals in RenderUtil::Update + /// \param[in] _ecm The entity-component manager + public: void PopulateViewModeVisualLinks(const EntityComponentManager &_ecm); + + /// \brief A list of links used to toggle wireframe mode for visuals + public: std::vector newWireframeVisualLinks; + + /// \brief A list of links used to toggle transparent mode for visuals + public: std::vector newTransparentVisualLinks; + + /// \brief A map of link entities and their corresponding children visuals + public: std::map> linkToVisualEntities; + + /// \brief A map of created wireframe visuals and if they are currently + /// visible + public: std::map viewingWireframes; + + /// \brief A map of created transparent visuals and if they are currently + /// visible + public: std::map viewingTransparent; + /// \brief New collisions to be created public: std::vector newCollisions; @@ -334,6 +505,11 @@ class ignition::gazebo::RenderUtilPrivate public: std::unordered_map> thermalCameraData; + /// \brief Update the visuals with label user data + /// \param[in] _entityLabel Map with key visual entity id and value label + public: void UpdateVisualLabels( + const std::unordered_map &_entityLabel); + /// \brief A helper function that removes the sensor associated with an /// entity, if an associated sensor exists. This should be called in /// RenderUtil::Update. @@ -495,6 +671,41 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, } return true; }); + + // visual commands + { + auto olderEntityVisualsCmdToDelete + = std::move(this->dataPtr->entityVisualsCmdToDelete); + this->dataPtr->entityVisualsCmdToDelete.clear(); + + // TODO(anyone) Currently only updates material colors, + // need to extend to others + _ecm.Each( + [&](const Entity &_entity, + const components::VisualCmd *_visualCmd) -> bool + { + this->dataPtr->entityVisuals[_entity] = _visualCmd->Data(); + this->dataPtr->entityVisualsCmdToDelete.push_back(_entity); + + auto materialComp = _ecm.Component(_entity); + if (materialComp) + { + msgs::Material materialMsg = _visualCmd->Data().material(); + sdf::Material sdfMaterial = convert(materialMsg); + + auto state = + materialComp->SetData(sdfMaterial, this->dataPtr->materialEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + _ecm.SetChanged(_entity, components::Material::typeId, state); + } + return true; + }); + + for (const auto entity : olderEntityVisualsCmdToDelete) + { + _ecm.RemoveComponent(entity); + } + } } ////////////////////////////////////////////////// @@ -509,35 +720,127 @@ void RenderUtil::UpdateFromECM(const UpdateInfo &_info, this->dataPtr->UpdateRenderingEntities(_ecm); this->dataPtr->RemoveRenderingEntities(_ecm, _info); this->dataPtr->markerManager.SetSimTime(_info.simTime); + this->dataPtr->PopulateViewModeVisualLinks(_ecm); + this->dataPtr->FindInertialLinks(_ecm); + this->dataPtr->FindJointModels(_ecm); this->dataPtr->FindCollisionLinks(_ecm); } ////////////////////////////////////////////////// -void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) +std::vector RenderUtilPrivate::FindChildLinksFromECM( + const EntityComponentManager &_ecm, const Entity &_entity) { - if (this->newCollisions.empty()) - return; + std::vector links; + if (_ecm.EntityMatches(_entity, + std::set{components::Model::typeId})) + { + std::stack modelStack; + modelStack.push(_entity); - for (const auto &entity : this->newCollisions) + std::vector childLinks, childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Link()); + links.insert(links.end(), + childLinks.begin(), + childLinks.end()); + + childModels = + _ecm.EntitiesByComponents(components::ParentEntity(model), + components::Model()); + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + else if (_ecm.EntityMatches(_entity, + std::set{components::Link::typeId})) + { + links.push_back(_entity); + } + return links; +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::FindInertialLinks(const EntityComponentManager &_ecm) +{ + for (const auto &entity : this->newInertias) { std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing inertia must be a model or link" + << std::endl; + continue; + } + + this->newInertiaLinks.insert(this->newInertiaLinks.end(), + links.begin(), + links.end()); + } + this->newInertias.clear(); + + for (const auto &entity : this->newCOMVisuals) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing center of mass must be a model or link" + << std::endl; + continue; + } + + this->newCOMLinks.insert(this->newCOMLinks.end(), + links.begin(), + links.end()); + } + this->newCOMVisuals.clear(); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::FindJointModels(const EntityComponentManager &_ecm) +{ + if (this->newJoints.empty()) + { + return; + } + + for (const auto &entity : this->newJoints) + { + std::vector models; if (_ecm.EntityMatches(entity, std::set{components::Model::typeId})) { std::stack modelStack; modelStack.push(entity); - std::vector childLinks, childModels; + std::vector childModels; while (!modelStack.empty()) { Entity model = modelStack.top(); modelStack.pop(); - - childLinks = _ecm.EntitiesByComponents(components::ParentEntity(model), - components::Link()); - links.insert(links.end(), - childLinks.begin(), - childLinks.end()); + models.push_back(model); childModels = _ecm.EntitiesByComponents(components::ParentEntity(model), @@ -548,10 +851,92 @@ void RenderUtilPrivate::FindCollisionLinks(const EntityComponentManager &_ecm) } } } - else if (_ecm.EntityMatches(entity, + else + { + ignerr << "Entity [" << entity + << "] for viewing joints must be a model" + << std::endl; + continue; + } + + this->newJointModels.insert(this->newJointModels.end(), + models.begin(), + models.end()); + } + this->newJoints.clear(); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::PopulateViewModeVisualLinks( + const EntityComponentManager &_ecm) +{ + // Find links to toggle wireframes + for (const auto &entity : this->newWireframes) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing wireframe must be a model or link" + << std::endl; + continue; + } + + this->newWireframeVisualLinks.insert(this->newWireframeVisualLinks.end(), + links.begin(), + links.end()); + } + this->newWireframes.clear(); + + // Find links to view as transparent + for (const auto &entity : this->newTransparentEntities) + { + std::vector links; + if (_ecm.EntityMatches(entity, + std::set{components::Model::typeId}) || + _ecm.EntityMatches(entity, + std::set{components::Link::typeId})) + { + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); + } + else + { + ignerr << "Entity [" << entity + << "] for viewing as transparent must be a model or link" + << std::endl; + continue; + } + + this->newTransparentVisualLinks.insert( + this->newTransparentVisualLinks.end(), + links.begin(), + links.end()); + } + this->newTransparentEntities.clear(); +} + +////////////////////////////////////////////////// +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}) || + _ecm.EntityMatches(entity, std::set{components::Link::typeId})) { - links.push_back(entity); + links = std::move(this->FindChildLinksFromECM(_ecm, entity)); } else { @@ -606,10 +991,21 @@ void RenderUtil::Update() auto removeEntities = std::move(this->dataPtr->removeEntities); auto entityPoses = std::move(this->dataPtr->entityPoses); auto entityLights = std::move(this->dataPtr->entityLights); + auto entityVisuals = std::move(this->dataPtr->entityVisuals); + auto updateJointParentPoses = + std::move(this->dataPtr->updateJointParentPoses); 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 entityLabel = std::move(this->dataPtr->entityLabel); + auto newTransparentVisualLinks = + std::move(this->dataPtr->newTransparentVisualLinks); + auto newInertiaLinks = std::move(this->dataPtr->newInertiaLinks); + auto newJointModels = std::move(this->dataPtr->newJointModels); + auto newCOMLinks = std::move(this->dataPtr->newCOMLinks); + auto newWireframeVisualLinks = + std::move(this->dataPtr->newWireframeVisualLinks); auto newCollisionLinks = std::move(this->dataPtr->newCollisionLinks); auto thermalCameraData = std::move(this->dataPtr->thermalCameraData); @@ -624,10 +1020,18 @@ void RenderUtil::Update() this->dataPtr->removeEntities.clear(); this->dataPtr->entityPoses.clear(); this->dataPtr->entityLights.clear(); + this->dataPtr->entityVisuals.clear(); + this->dataPtr->updateJointParentPoses.clear(); this->dataPtr->trajectoryPoses.clear(); this->dataPtr->actorTransforms.clear(); this->dataPtr->actorAnimationData.clear(); this->dataPtr->entityTemp.clear(); + this->dataPtr->entityLabel.clear(); + this->dataPtr->newTransparentVisualLinks.clear(); + this->dataPtr->newInertiaLinks.clear(); + this->dataPtr->newJointModels.clear(); + this->dataPtr->newCOMLinks.clear(); + this->dataPtr->newWireframeVisualLinks.clear(); this->dataPtr->newCollisionLinks.clear(); this->dataPtr->thermalCameraData.clear(); @@ -645,8 +1049,16 @@ void RenderUtil::Update() // extend the sensor system to support mutliple scenes in the future for (auto &scene : newScenes) { - this->dataPtr->scene->SetAmbientLight(scene.Ambient()); - this->dataPtr->scene->SetBackgroundColor(scene.Background()); + // Only set the ambient color if the RenderUtil::SetBackgroundColor + // was not called. + if (!this->dataPtr->ambientLight) + this->dataPtr->scene->SetAmbientLight(scene.Ambient()); + + // Only set the background color if the RenderUtil::SetBackgroundColor + // was not called. + if (!this->dataPtr->backgroundColor) + this->dataPtr->scene->SetBackgroundColor(scene.Background()); + if (scene.Grid() && !this->dataPtr->enableSensors) this->ShowGrid(); if (scene.Sky()) @@ -714,13 +1126,14 @@ void RenderUtil::Update() for (const auto &actor : newActors) { this->dataPtr->sceneManager.CreateActor( - std::get<0>(actor), std::get<1>(actor), std::get<2>(actor)); + std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), + std::get<3>(actor)); } for (const auto &light : newLights) { - this->dataPtr->sceneManager.CreateLight( - std::get<0>(light), std::get<1>(light), std::get<2>(light)); + this->dataPtr->sceneManager.CreateLight(std::get<0>(light), + std::get<1>(light), std::get<2>(light), std::get<3>(light)); // TODO(anyone) This needs to be updated for when sensors and GUI use // the same scene @@ -735,7 +1148,7 @@ void RenderUtil::Update() { rendering::VisualPtr lightVisual = this->dataPtr->sceneManager.CreateLightVisual( - id, std::get<1>(light), std::get<0>(light)); + id, std::get<1>(light), std::get<2>(light), std::get<0>(light)); this->dataPtr->matchLightWithVisuals[std::get<0>(light)] = id; break; } @@ -877,8 +1290,7 @@ void RenderUtil::Update() if (!node) continue; - auto visual = - std::dynamic_pointer_cast(node); + auto visual = std::dynamic_pointer_cast(node); if (!visual) continue; @@ -893,534 +1305,911 @@ void RenderUtil::Update() } } - // create new collision visuals + this->dataPtr->UpdateVisualLabels(entityLabel); + + // update joint parent visual poses { - for (const auto &link : newCollisionLinks) + for (const auto &jointEntity : updateJointParentPoses) { - std::vector colEntities = - this->dataPtr->linkToCollisionEntities[link]; + this->dataPtr->sceneManager.UpdateJointParentPose(jointEntity); + } + } - for (const auto &colEntity : colEntities) + // create new transparent visuals + { + for (const auto &link : newTransparentVisualLinks) + { + std::vector visEntities = + this->dataPtr->linkToVisualEntities[link]; + + for (const auto &visEntity : visEntities) { - if (!this->dataPtr->sceneManager.HasEntity(colEntity)) + if (!this->dataPtr->viewingTransparent[visEntity]) { - 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); + auto vis = this->dataPtr->sceneManager.VisualById(visEntity); - // 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(); - } - } + this->dataPtr->sceneManager.UpdateTransparency(vis, + true /* transparent */); + this->dataPtr->viewingTransparent[visEntity] = true; } } } } - this->dataPtr->UpdateThermalCamera(thermalCameraData); -} - -////////////////////////////////////////////////// -void RenderUtilPrivate::CreateRenderingEntities( - const EntityComponentManager &_ecm, const UpdateInfo &_info) -{ - IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); - auto addNewSensor = [&_ecm, this](Entity _entity, const sdf::Sensor &_sdfData, - Entity _parent, - const std::string &_topicSuffix) + // create new inertia visuals { - sdf::Sensor sdfDataCopy(_sdfData); - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdfDataCopy.SetName(sensorScopedName); - // check topic - if (sdfDataCopy.Topic().empty()) + for (const auto &link : newInertiaLinks) { - sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); + // create a new id for the inertia visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->sceneManager.HasEntity(id) && + !this->dataPtr->viewingInertias[link]) + { + rendering::VisualPtr inrVisual = + this->dataPtr->sceneManager.CreateInertiaVisual( + id, this->dataPtr->entityInertials[link], link); + this->dataPtr->viewingInertias[link] = true; + this->dataPtr->linkToInertiaVisuals[link] = id; + break; + } + } } - this->newSensors.push_back( - std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); - this->sensorEntities.insert(_entity); - }; - - const std::string cameraSuffix{"/image"}; - const std::string depthCameraSuffix{"/depth_image"}; - const std::string rgbdCameraSuffix{""}; - const std::string thermalCameraSuffix{"/image"}; - const std::string gpuLidarSuffix{"/scan"}; + } - // Treat all pre-existent entities as new at startup - // TODO(anyone) refactor Each and EachNew below to reduce duplicate code - if (!this->initialized) + // create new joint visuals { - // Get all the new worlds - // TODO(anyone) Only one scene is supported for now - // extend the sensor system to support mutliple scenes in the future - _ecm.Each( - [&](const Entity & _entity, - const components::World *, - const components::Scene *_scene)->bool + for (const auto &model : newJointModels) + { + std::vector jointEntities = + this->dataPtr->modelToJointEntities[model]; + + for (const auto &jointEntity : jointEntities) + { + if (!this->dataPtr->sceneManager.HasEntity(jointEntity)) { - this->sceneManager.SetWorldId(_entity); - const sdf::Scene &sceneSdf = _scene->Data(); - this->newScenes.push_back(sceneSdf); - return true; - }); + std::string childLinkName = + this->dataPtr->entityJoints[jointEntity].ChildLinkName(); + Entity childId = + this->dataPtr->matchLinksWithEntities[model][childLinkName]; + std::string parentLinkName = + this->dataPtr->entityJoints[jointEntity].ParentLinkName(); + Entity parentId = + this->dataPtr->matchLinksWithEntities[model][parentLinkName]; - _ecm.Each( - [&](const Entity &_entity, - const components::Model *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool - { - sdf::Model model; - model.SetName(_name->Data()); - model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); - return true; - }); + auto joint = this->dataPtr->entityJoints[jointEntity]; + + auto vis = this->dataPtr->sceneManager.CreateJointVisual( + jointEntity, joint, childId, parentId); + this->dataPtr->viewingJoints[jointEntity] = true; + + // Update joint parent visual pose + if (joint.Axis(1)) + { + this->dataPtr->updateJointParentPoses.push_back(jointEntity); + } + } + } + } + } - _ecm.Each( - [&](const Entity &_entity, - const components::Link *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + // create new center of mass visuals + { + for (const auto &link : newCOMLinks) + { + // create a new id for the center of mass visual + auto attempts = 100000u; + for (auto i = 0u; i < attempts; ++i) + { + Entity id = std::numeric_limits::max() - i; + if (!this->dataPtr->sceneManager.HasEntity(id) && + !this->dataPtr->viewingCOM[link]) { - sdf::Link link; - link.SetName(_name->Data()); - 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; - }); + rendering::VisualPtr inrVisual = + this->dataPtr->sceneManager.CreateCOMVisual( + id, this->dataPtr->entityInertials[link], link); + this->dataPtr->viewingCOM[link] = true; + this->dataPtr->linkToCOMVisuals[link] = id; + break; + } + } + } + } - // visuals - _ecm.Each( - [&](const Entity &_entity, - const components::Visual *, - const components::Name *_name, - const components::Pose *_pose, - const components::Geometry *_geom, - const components::CastShadows *_castShadows, - const components::Transparency *_transparency, - const components::VisibilityFlags *_visibilityFlags, - const components::ParentEntity *_parent)->bool + // create new wireframe visuals + { + for (const auto &link : newWireframeVisualLinks) + { + std::vector visEntities = + this->dataPtr->linkToVisualEntities[link]; + + for (const auto &visEntity : visEntities) + { + if (!this->dataPtr->viewingWireframes[visEntity]) { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) + auto vis = this->dataPtr->sceneManager.VisualById(visEntity); + vis->SetWireframe(true); + this->dataPtr->viewingWireframes[visEntity] = true; + } + } + } + } + + // 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) { - visual.SetMaterial(material->Data()); + 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(); + } } + } + } + } + } + + this->dataPtr->UpdateThermalCamera(thermalCameraData); - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) + // update visuals + // TODO(anyone) currently updates material colors of visual only, + // need to extend to other updates + { + IGN_PROFILE("RenderUtil::Update Visuals"); + for (const auto &visual : entityVisuals) + { + if (!visual.second.has_material()) + continue; + + auto node = this->dataPtr->sceneManager.NodeById(visual.first); + if (!node) + continue; + + auto vis = std::dynamic_pointer_cast(node); + if (vis) + { + msgs::Material matMsg = visual.second.material(); + + // Geometry material + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + rendering::GeometryPtr geom = vis->GeometryByIndex(g); + rendering::MaterialPtr geomMat = geom->Material(); + if (!geomMat) + continue; + + math::Color color; + if (matMsg.has_ambient()) { - visual.SetLaserRetro(laserRetro->Data()); + color = msgs::Convert(matMsg.ambient()); + if (geomMat->Ambient() != color) + geomMat->SetAmbient(color); } - if (auto temp = _ecm.Component(_entity)) + if (matMsg.has_diffuse()) { - // get the uniform temperature for the entity - this->entityTemp[_entity] = - std::make_tuple( - temp->Data().Kelvin(), 0.0, ""); + color = msgs::Convert(matMsg.diffuse()); + if (geomMat->Diffuse() != color) + geomMat->SetDiffuse(color); } - else + + if (matMsg.has_specular()) { - // 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())); - } + color = msgs::Convert(matMsg.specular()); + if (geomMat->Specular() != color) + geomMat->SetSpecular(color); } - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); - return true; - }); + if (matMsg.has_emissive()) + { + color = msgs::Convert(matMsg.emissive()); + if (geomMat->Emissive() != color) + geomMat->SetEmissive(color); + } + } + } + } + } +} - // actors - _ecm.Each( - [&](const Entity &_entity, - const components::Actor *_actor, - const components::ParentEntity *_parent) -> bool +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateRenderingEntities( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + IGN_PROFILE("RenderUtilPrivate::CreateRenderingEntities"); + + // Treat all pre-existent entities as new at startup + // TODO(anyone) Combine the two CreateEntities functions below to reduce + // duplicate code + if (!this->initialized) + { + this->CreateEntitiesFirstUpdate(_ecm, _info); + this->initialized = true; + } + else + { + this->CreateEntitiesRuntime(_ecm, _info); + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm, + Entity _entity, const sdf::Sensor &_sdfData, Entity _parent, + const std::string &_topicSuffix) +{ + sdf::Sensor sdfDataCopy(_sdfData); + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdfDataCopy.SetName(sensorScopedName); + // check topic + if (sdfDataCopy.Topic().empty()) + { + sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); + } + this->newSensors.push_back( + std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); + this->sensorEntities.insert(_entity); +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateEntitiesFirstUpdate( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + const std::string cameraSuffix{"/image"}; + const std::string depthCameraSuffix{"/depth_image"}; + const std::string rgbdCameraSuffix{""}; + const std::string thermalCameraSuffix{"/image"}; + const std::string gpuLidarSuffix{"/scan"}; + const std::string segmentationCameraSuffix{"/segmentation"}; + + // Get all the new worlds + // TODO(anyone) Only one scene is supported for now + // extend the sensor system to support mutliple scenes in the future + _ecm.Each( + [&](const Entity & _entity, + const components::World *, + const components::Scene *_scene)->bool + { + this->sceneManager.SetWorldId(_entity); + const sdf::Scene &sceneSdf = _scene->Data(); + this->newScenes.push_back(sceneSdf); + return true; + }); + + + _ecm.Each( + [&](const Entity &_entity, + const components::Model *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Model model; + model.SetName(_name->Data()); + model.SetRawPose(_pose->Data()); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + return true; + }); + + _ecm.Each( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Link link; + link.SetName(_name->Data()); + 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); + // used for joints + this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // visuals + _ecm.Each( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent)->bool + { + sdf::Visual visual; + visual.SetName(_name->Data()); + visual.SetRawPose(_pose->Data()); + visual.SetGeom(_geom->Data()); + visual.SetCastShadows(_castShadows->Data()); + visual.SetTransparency(_transparency->Data()); + visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // Optional components + auto material = _ecm.Component(_entity); + if (material != nullptr) { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); - return true; - }); + visual.SetMaterial(material->Data()); + } + + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) + { + visual.SetLaserRetro(laserRetro->Data()); + } + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->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())); + + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + return true; + }); - // lights - _ecm.Each( - [&](const Entity &_entity, - const components::Light *_light, - const components::ParentEntity *_parent) -> bool + // actors + _ecm.Each( + [&](const Entity &_entity, + const components::Actor *_actor, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newActors.push_back(std::make_tuple(_entity, _actor->Data(), + _name->Data(), _parent->Data())); + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + this->entityLabel[_entity] = label->Data(); + } + + return true; + }); + + // lights + _ecm.Each( + [&](const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); + return true; + }); + + // inertials + _ecm.Each( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + 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; + }); + + // joints + _ecm.Each( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) + { + joint.SetAxis(1, jointAxis2->Data()); + } + + this->entityJoints[_entity] = joint; + this->modelToJointEntities[_parentModel->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 + _ecm.Each( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), + cameraSuffix); 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 + // Create depth cameras + _ecm.Each( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool { - this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), + _parent->Data(), depthCameraSuffix); return true; }); - // particle emitters - _ecm.Each( - [&](const Entity &_entity, - const components::ParticleEmitter *_emitter, - const components::ParentEntity *_parent) -> bool + // Create rgbd cameras + _ecm.Each( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), + _parent->Data(), rgbdCameraSuffix); return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.Each( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.Each( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create rgbd cameras - _ecm.Each( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.Each( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.Each( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } - this->initialized = true; - } - else - { - // Get all the new worlds - // TODO(anyone) Only one scene is supported for now - // extend the sensor system to support mutliple scenes in the future - _ecm.EachNew( - [&](const Entity & _entity, - const components::World *, - const components::Scene *_scene)->bool + // Create gpu lidar + _ecm.Each( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool { - this->sceneManager.SetWorldId(_entity); - const sdf::Scene &sceneSdf = _scene->Data(); - this->newScenes.push_back(sceneSdf); + this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); return true; }); - _ecm.EachNew( - [&](const Entity &_entity, - const components::Model *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + // Create thermal camera + _ecm.Each( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool { - sdf::Model model; - model.SetName(_name->Data()); - model.SetRawPose(_pose->Data()); - this->newModels.push_back( - std::make_tuple(_entity, model, _parent->Data(), - _info.iterations)); - this->modelToModelEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), + _parent->Data(), thermalCameraSuffix); return true; }); - _ecm.EachNew( - [&](const Entity &_entity, - const components::Link *, - const components::Name *_name, - const components::Pose *_pose, - const components::ParentEntity *_parent)->bool + // Create segmentation cameras + _ecm.Each( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool { - sdf::Link link; - link.SetName(_name->Data()); - 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); + this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); return true; }); + } +} + +////////////////////////////////////////////////// +void RenderUtilPrivate::CreateEntitiesRuntime( + const EntityComponentManager &_ecm, const UpdateInfo &_info) +{ + const std::string cameraSuffix{"/image"}; + const std::string depthCameraSuffix{"/depth_image"}; + const std::string rgbdCameraSuffix{""}; + const std::string thermalCameraSuffix{"/image"}; + const std::string gpuLidarSuffix{"/scan"}; + const std::string segmentationCameraSuffix{"/segmentation"}; + + // Get all the new worlds + // TODO(anyone) Only one scene is supported for now + // extend the sensor system to support mutliple scenes in the future + _ecm.EachNew( + [&](const Entity & _entity, + const components::World *, + const components::Scene *_scene)->bool + { + this->sceneManager.SetWorldId(_entity); + const sdf::Scene &sceneSdf = _scene->Data(); + this->newScenes.push_back(sceneSdf); + return true; + }); + + _ecm.EachNew( + [&](const Entity &_entity, + const components::Model *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Model model; + model.SetName(_name->Data()); + model.SetRawPose(_pose->Data()); + this->newModels.push_back(std::make_tuple(_entity, model, + _parent->Data(), _info.iterations)); + this->modelToModelEntities[_parent->Data()].push_back(_entity); + return true; + }); - // visuals - _ecm.EachNew( - [&](const Entity &_entity, - const components::Visual *, - const components::Name *_name, - const components::Pose *_pose, - const components::Geometry *_geom, - const components::CastShadows *_castShadows, - const components::Transparency *_transparency, - const components::VisibilityFlags *_visibilityFlags, - const components::ParentEntity *_parent)->bool + _ecm.EachNew( + [&](const Entity &_entity, + const components::Link *, + const components::Name *_name, + const components::Pose *_pose, + const components::ParentEntity *_parent)->bool + { + sdf::Link link; + link.SetName(_name->Data()); + 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); + // used for joints + this->matchLinksWithEntities[_parent->Data()][_name->Data()] = + _entity; + return true; + }); + + // visuals + _ecm.EachNew( + [&](const Entity &_entity, + const components::Visual *, + const components::Name *_name, + const components::Pose *_pose, + const components::Geometry *_geom, + const components::CastShadows *_castShadows, + const components::Transparency *_transparency, + const components::VisibilityFlags *_visibilityFlags, + const components::ParentEntity *_parent)->bool + { + sdf::Visual visual; + visual.SetName(_name->Data()); + visual.SetRawPose(_pose->Data()); + visual.SetGeom(_geom->Data()); + visual.SetCastShadows(_castShadows->Data()); + visual.SetTransparency(_transparency->Data()); + visual.SetVisibilityFlags(_visibilityFlags->Data()); + + // Optional components + auto material = _ecm.Component(_entity); + if (material != nullptr) { - sdf::Visual visual; - visual.SetName(_name->Data()); - visual.SetRawPose(_pose->Data()); - visual.SetGeom(_geom->Data()); - visual.SetCastShadows(_castShadows->Data()); - visual.SetTransparency(_transparency->Data()); - visual.SetVisibilityFlags(_visibilityFlags->Data()); - - // Optional components - auto material = _ecm.Component(_entity); - if (material != nullptr) - { - visual.SetMaterial(material->Data()); - } + visual.SetMaterial(material->Data()); + } - auto laserRetro = _ecm.Component(_entity); - if (laserRetro != nullptr) - { - visual.SetLaserRetro(laserRetro->Data()); - } + auto laserRetro = _ecm.Component(_entity); + if (laserRetro != nullptr) + { + visual.SetLaserRetro(laserRetro->Data()); + } - if (auto temp = _ecm.Component(_entity)) + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->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) { - // 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())); - } + tempRange->Data().min.Kelvin(), + tempRange->Data().max.Kelvin(), + std::string(heatSignature->Data())); } + } + + this->newVisuals.push_back( + std::make_tuple(_entity, visual, _parent->Data())); + + this->linkToVisualEntities[_parent->Data()].push_back(_entity); + return true; + }); - this->newVisuals.push_back( - std::make_tuple(_entity, visual, _parent->Data())); + // actors + _ecm.EachNew( + [&](const Entity &_entity, + const components::Actor *_actor, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newActors.push_back( + std::make_tuple(_entity, _actor->Data(), _name->Data(), + _parent->Data())); + + // set label + auto label = _ecm.Component(_entity); + if (label != nullptr) + { + this->entityLabel[_entity] = label->Data(); + } + + return true; + }); + + // lights + _ecm.EachNew( + [&](const Entity &_entity, + const components::Light *_light, + const components::Name *_name, + const components::ParentEntity *_parent) -> bool + { + this->newLights.push_back(std::make_tuple(_entity, _light->Data(), + _name->Data(), _parent->Data())); + return true; + }); + + // inertials + _ecm.EachNew( + [&](const Entity &_entity, + const components::Inertial *_inrElement, + const components::Pose *) -> bool + { + this->entityInertials[_entity] = _inrElement->Data(); + 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; + }); + + // joints + _ecm.EachNew( + [&](const Entity &_entity, + const components::Joint * /* _joint */, + const components::Name *_name, + const components::JointType *_jointType, + const components::Pose *_pose, + const components::ParentEntity *_parentModel, + const components::ParentLinkName *_parentLinkName, + const components::ChildLinkName *_childLinkName) -> bool + { + sdf::Joint joint; + joint.SetName(_name->Data()); + joint.SetType(_jointType->Data()); + joint.SetRawPose(_pose->Data()); + + joint.SetParentLinkName(_parentLinkName->Data()); + joint.SetChildLinkName(_childLinkName->Data()); + + auto jointAxis = _ecm.Component(_entity); + auto jointAxis2 = _ecm.Component(_entity); + + if (jointAxis) + { + joint.SetAxis(0, jointAxis->Data()); + } + if (jointAxis2) + { + joint.SetAxis(1, jointAxis2->Data()); + } + + this->entityJoints[_entity] = joint; + this->modelToJointEntities[_parentModel->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 + _ecm.EachNew( + [&](const Entity &_entity, + const components::Camera *_camera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _camera->Data(), _parent->Data(), + cameraSuffix); return true; }); - // actors - _ecm.EachNew( - [&](const Entity &_entity, - const components::Actor *_actor, - const components::ParentEntity *_parent) -> bool + // Create depth cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::DepthCamera *_depthCamera, + const components::ParentEntity *_parent)->bool { - this->newActors.push_back( - std::make_tuple(_entity, _actor->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _depthCamera->Data(), + _parent->Data(), depthCameraSuffix); return true; }); - // lights - _ecm.EachNew( - [&](const Entity &_entity, - const components::Light *_light, - const components::ParentEntity *_parent) -> bool + // Create RGBD cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::RgbdCamera *_rgbdCamera, + const components::ParentEntity *_parent)->bool { - this->newLights.push_back( - std::make_tuple(_entity, _light->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _rgbdCamera->Data(), + _parent->Data(), rgbdCameraSuffix); 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 + // Create gpu lidar + _ecm.EachNew( + [&](const Entity &_entity, + const components::GpuLidar *_gpuLidar, + const components::ParentEntity *_parent)->bool { - this->entityCollisions[_entity] = _collElement->Data(); - this->linkToCollisionEntities[_parent->Data()].push_back(_entity); + this->AddNewSensor(_ecm, _entity, _gpuLidar->Data(), _parent->Data(), + gpuLidarSuffix); return true; }); - // particle emitters - _ecm.EachNew( - [&](const Entity &_entity, - const components::ParticleEmitter *_emitter, - const components::ParentEntity *_parent) -> bool + // Create thermal camera + _ecm.EachNew( + [&](const Entity &_entity, + const components::ThermalCamera *_thermalCamera, + const components::ParentEntity *_parent)->bool { - this->newParticleEmitters.push_back( - std::make_tuple(_entity, _emitter->Data(), _parent->Data())); + this->AddNewSensor(_ecm, _entity, _thermalCamera->Data(), + _parent->Data(), thermalCameraSuffix); return true; }); - if (this->enableSensors) - { - // Create cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::Camera *_camera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _camera->Data(), _parent->Data(), - cameraSuffix); - return true; - }); - - // Create depth cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::DepthCamera *_depthCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _depthCamera->Data(), _parent->Data(), - depthCameraSuffix); - return true; - }); - - // Create RGBD cameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::RgbdCamera *_rgbdCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _rgbdCamera->Data(), _parent->Data(), - rgbdCameraSuffix); - return true; - }); - - // Create gpu lidar - _ecm.EachNew( - [&](const Entity &_entity, - const components::GpuLidar *_gpuLidar, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _gpuLidar->Data(), _parent->Data(), - gpuLidarSuffix); - return true; - }); - - // Create thermal camera - _ecm.EachNew( - [&](const Entity &_entity, - const components::ThermalCamera *_thermalCamera, - const components::ParentEntity *_parent)->bool - { - addNewSensor(_entity, _thermalCamera->Data(), _parent->Data(), - thermalCameraSuffix); - return true; - }); - } + // Create segmentation cameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::SegmentationCamera *_segmentationCamera, + const components::ParentEntity *_parent)->bool + { + this->AddNewSensor(_ecm, _entity, _segmentationCamera->Data(), + _parent->Data(), segmentationCameraSuffix); + return true; + }); } } @@ -1571,6 +2360,16 @@ void RenderUtilPrivate::UpdateRenderingEntities( this->entityPoses[_entity] = _pose->Data(); return true; }); + + // Update segmentation cameras + _ecm.Each( + [&](const Entity &_entity, + const components::SegmentationCamera *, + const components::Pose *_pose)->bool + { + this->entityPoses[_entity] = _pose->Data(); + return true; + }); } ////////////////////////////////////////////////// @@ -1584,6 +2383,7 @@ void RenderUtilPrivate::RemoveRenderingEntities( this->removeEntities[_entity] = _info.iterations; this->modelToLinkEntities.erase(_entity); this->modelToModelEntities.erase(_entity); + this->matchLinksWithEntities.erase(_entity); return true; }); @@ -1591,7 +2391,28 @@ void RenderUtilPrivate::RemoveRenderingEntities( [&](const Entity &_entity, const components::Link *)->bool { this->removeEntities[_entity] = _info.iterations; + this->linkToVisualEntities.erase(_entity); this->linkToCollisionEntities.erase(_entity); + + if (this->linkToInertiaVisuals.find(_entity) != + this->linkToInertiaVisuals.end()) + { + this->removeEntities[this->linkToInertiaVisuals[_entity]] = + _info.iterations; + } + + if (this->linkToCOMVisuals.find(_entity) != + this->linkToCOMVisuals.end()) + { + this->removeEntities[this->linkToCOMVisuals[_entity]] = + _info.iterations; + } + + this->linkToInertiaVisuals.erase(_entity); + this->viewingInertias.erase(_entity); + this->linkToCOMVisuals.erase(_entity); + this->viewingCOM.erase(_entity); + this->entityInertials.erase(_entity); return true; }); @@ -1614,6 +2435,16 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // joints + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Joint *)->bool + { + this->removeEntities[_entity] = _info.iterations; + this->entityJoints.erase(_entity); + this->viewingJoints.erase(_entity); + return true; + }); + // particle emitters _ecm.EachRemoved( [&](const Entity &_entity, const components::ParticleEmitter *)->bool @@ -1662,6 +2493,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // segmentation cameras + _ecm.EachRemoved( + [&](const Entity &_entity, const components::SegmentationCamera *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // collisions _ecm.EachRemoved( [&](const Entity &_entity, const components::Collision *)->bool @@ -1673,9 +2512,25 @@ void RenderUtilPrivate::RemoveRenderingEntities( }); } +///////////////////////////////////////////////// +void RenderUtil::SetHeadlessRendering(const bool &_headless) +{ + this->dataPtr->isHeadlessRendering = _headless; +} + +///////////////////////////////////////////////// +bool RenderUtil::HeadlessRendering() const +{ + return this->dataPtr->isHeadlessRendering; +} + ///////////////////////////////////////////////// void RenderUtil::Init() { + // Already initialized + if (nullptr != this->dataPtr->scene) + return; + ignition::common::SystemPaths pluginPath; pluginPath.SetPluginPathEnv(kRenderPluginPathEnv); rendering::setPluginPaths(pluginPath.PluginPaths()); @@ -1683,6 +2538,9 @@ void RenderUtil::Init() std::map params; if (this->dataPtr->useCurrentGLContext) params["useCurrentGLContext"] = "1"; + if (this->dataPtr->isHeadlessRendering) + params["headless"] = "1"; + params["winID"] = this->dataPtr->winID; this->dataPtr->engine = rendering::engine(this->dataPtr->engineName, params); if (!this->dataPtr->engine) { @@ -1701,8 +2559,17 @@ void RenderUtil::Init() this->dataPtr->engine->CreateScene(this->dataPtr->sceneName); if (this->dataPtr->scene) { - this->dataPtr->scene->SetAmbientLight(this->dataPtr->ambientLight); - this->dataPtr->scene->SetBackgroundColor(this->dataPtr->backgroundColor); + if (this->dataPtr->ambientLight) + { + this->dataPtr->scene->SetAmbientLight( + *this->dataPtr->ambientLight); + } + + if (this->dataPtr->backgroundColor) + { + this->dataPtr->scene->SetBackgroundColor( + *this->dataPtr->backgroundColor); + } this->dataPtr->scene->SetSkyEnabled(this->dataPtr->skyEnabled); } } @@ -1776,6 +2643,14 @@ void RenderUtil::SetSceneName(const std::string &_name) this->dataPtr->sceneName = _name; } +///////////////////////////////////////////////// +void RenderUtil::SetScene(const rendering::ScenePtr &_scene) +{ + this->dataPtr->scene = _scene; + this->dataPtr->sceneManager.SetScene(_scene); + this->dataPtr->engine = _scene == nullptr ? nullptr : _scene->Engine(); +} + ///////////////////////////////////////////////// std::string RenderUtil::SceneName() const { @@ -1794,6 +2669,12 @@ void RenderUtil::SetUseCurrentGLContext(bool _enable) this->dataPtr->useCurrentGLContext = _enable; } +///////////////////////////////////////////////// +void RenderUtil::SetWinID(const std::string &_winID) +{ + this->dataPtr->winID = _winID; +} + ///////////////////////////////////////////////// void RenderUtil::SetEnableSensors(bool _enable, std::functiondataPtr->originalEmissive.clear(); } -///////////////////////////////////////////////// -rendering::NodePtr RenderUtil::SelectedEntity() const -{ - // Return most recently selected node - auto node = this->dataPtr->sceneManager.NodeById( - this->dataPtr->selectedEntities.back()); - return node; -} - ///////////////////////////////////////////////// const std::vector &RenderUtil::SelectedEntities() const { @@ -1881,6 +2753,25 @@ void RenderUtil::SetTransformActive(bool _active) this->dataPtr->transformActive = _active; } +//////////////////////////////////////////////// +void RenderUtilPrivate::UpdateVisualLabels( + const std::unordered_map &_entityLabel) +{ + // set visual label + for (const auto &label : _entityLabel) + { + auto node = this->sceneManager.NodeById(label.first); + if (!node) + continue; + + auto visual = std::dynamic_pointer_cast(node); + if (!visual) + continue; + + visual->SetUserData("label", label.second); + } +} + //////////////////////////////////////////////// void RenderUtilPrivate::HighlightNode(const rendering::NodePtr &_node) { @@ -1921,6 +2812,7 @@ void RenderUtilPrivate::HighlightNode(const rendering::NodePtr &_node) wireBoxVis->SetInheritScale(false); wireBoxVis->AddGeometry(wireBox); wireBoxVis->SetMaterial(white, false); + wireBoxVis->SetUserData("gui-only", static_cast(true)); vis->AddChild(wireBoxVis); // Add wire box to map for setting visibility @@ -2216,17 +3108,11 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_map RenderUtil::FindChildLinks(const Entity &_entity) { - std::vector colEntities; std::vector links; - if (this->dataPtr->linkToCollisionEntities.find(_entity) != - this->dataPtr->linkToCollisionEntities.end()) - { - colEntities = this->dataPtr->linkToCollisionEntities[_entity]; - } - else if (this->dataPtr->modelToLinkEntities.find(_entity) != + if (this->dataPtr->modelToLinkEntities.find(_entity) != this->dataPtr->modelToLinkEntities.end()) { links.insert(links.end(), @@ -2258,14 +3144,392 @@ void RenderUtil::ViewCollisions(const Entity &_entity) } } + return links; +} + +///////////////////////////////////////////////// +void RenderUtil::HideWireboxes(const Entity &_entity) +{ + auto wireBoxIt = this->dataPtr->wireBoxes.find(_entity); + if (wireBoxIt != this->dataPtr->wireBoxes.end()) + { + ignition::rendering::WireBoxPtr wireBox = wireBoxIt->second; + auto visParent = wireBox->Parent(); + if (visParent) + visParent->SetVisible(false); + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewInertia(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->dataPtr->entityInertials.find(_entity) != + this->dataPtr->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle inertia visuals + bool showInertia, showInertiaInit = false; + // first loop looks for new inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingInertias.find(inertiaLink) == + this->dataPtr->viewingInertias.end()) + { + this->dataPtr->newInertias.push_back(_entity); + showInertiaInit = showInertia = true; + } + } + + // second loop toggles already created inertias + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingInertias.find(inertiaLink) == + this->dataPtr->viewingInertias.end()) + continue; + + // when viewing multiple inertias (e.g. _entity is a model), + // boolean for view inertias is based on first inrEntity in list + if (!showInertiaInit) + { + showInertia = !this->dataPtr->viewingInertias[inertiaLink]; + showInertiaInit = true; + } + + Entity inertiaVisualId = this->dataPtr->linkToInertiaVisuals[inertiaLink]; + rendering::VisualPtr inertiaVisual = + this->dataPtr->sceneManager.VisualById(inertiaVisualId); + if (inertiaVisual == nullptr) + { + ignerr << "Could not find inertia visual for entity [" << inertiaLink + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingInertias[inertiaLink] = showInertia; + inertiaVisual->SetVisible(showInertia); + + if (showInertia) + { + // turn off wireboxes for inertia visual entity + this->HideWireboxes(inertiaVisualId); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewCOM(const Entity &_entity) +{ + std::vector inertiaLinks = std::move(this->FindChildLinks(_entity)); + + // check if _entity has an inertial component (_entity is a link) + if (this->dataPtr->entityInertials.find(_entity) != + this->dataPtr->entityInertials.end()) + inertiaLinks.push_back(_entity); + + // create and/or toggle center of mass visuals + bool showCOM, showCOMInit = false; + // first loop looks for new center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingCOM.find(inertiaLink) == + this->dataPtr->viewingCOM.end()) + { + this->dataPtr->newCOMVisuals.push_back(_entity); + showCOMInit = showCOM = true; + } + } + + // second loop toggles already created center of mass visuals + for (const auto &inertiaLink : inertiaLinks) + { + if (this->dataPtr->viewingCOM.find(inertiaLink) == + this->dataPtr->viewingCOM.end()) + continue; + + // when viewing multiple center of mass visuals (e.g. _entity is a model), + // boolean for view center of mass is based on first inertiaEntity in list + if (!showCOMInit) + { + showCOM = !this->dataPtr->viewingCOM[inertiaLink]; + showCOMInit = true; + } + + Entity comVisualId = this->dataPtr->linkToCOMVisuals[inertiaLink]; + rendering::VisualPtr comVisual = + this->dataPtr->sceneManager.VisualById(comVisualId); + if (comVisual == nullptr) + { + ignerr << "Could not find center of mass visual for entity [" + << inertiaLink + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingCOM[inertiaLink] = showCOM; + comVisual->SetVisible(showCOM); + + if (showCOM) + { + // turn off wireboxes for CoM visual entity + this->HideWireboxes(comVisualId); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewJoints(const Entity &_entity) +{ + std::vector jointEntities; + if (this->dataPtr->modelToJointEntities.find(_entity) != + this->dataPtr->modelToJointEntities.end()) + { + jointEntities.insert(jointEntities.end(), + this->dataPtr->modelToJointEntities[_entity].begin(), + this->dataPtr->modelToJointEntities[_entity].end()); + } + + if (this->dataPtr->modelToModelEntities.find(_entity) != + this->dataPtr->modelToModelEntities.end()) + { + std::stack modelStack; + modelStack.push(_entity); + + std::vector childModels; + while (!modelStack.empty()) + { + Entity model = modelStack.top(); + modelStack.pop(); + + jointEntities.insert(jointEntities.end(), + this->dataPtr->modelToJointEntities[model].begin(), + this->dataPtr->modelToJointEntities[model].end()); + + childModels = this->dataPtr->modelToModelEntities[model]; + for (const auto &childModel : childModels) + { + modelStack.push(childModel); + } + } + } + + // Toggle joints + bool showJoint, showJointInit = false; + + // first loop looks for new joints + for (const auto &jointEntity : jointEntities) + { + if (this->dataPtr->viewingJoints.find(jointEntity) == + this->dataPtr->viewingJoints.end()) + { + this->dataPtr->newJoints.push_back(_entity); + showJointInit = showJoint = true; + } + } + + // second loop toggles joints + for (const auto &jointEntity : jointEntities) + { + if (this->dataPtr->viewingJoints.find(jointEntity) == + this->dataPtr->viewingJoints.end()) + continue; + + // when viewing multiple joints (e.g. _entity is a model), + // boolean for view joints is based on first jointEntity in list + if (!showJointInit) + { + showJoint = !this->dataPtr->viewingJoints[jointEntity]; + showJointInit = true; + } + + rendering::VisualPtr jointVisual = + this->dataPtr->sceneManager.VisualById(jointEntity); + if (jointVisual == nullptr) + { + ignerr << "Could not find visual for entity [" << jointEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingJoints[jointEntity] = showJoint; + jointVisual->SetVisible(showJoint); + + if (showJoint) + { + // turn off wireboxes for joint visual entity + this->HideWireboxes(jointEntity); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewTransparent(const Entity &_entity) +{ + std::vector visEntities; + + if (this->dataPtr->linkToVisualEntities.find(_entity) != + this->dataPtr->linkToVisualEntities.end()) + { + visEntities = this->dataPtr->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + visEntities.insert(visEntities.end(), + this->dataPtr->linkToVisualEntities[link].begin(), + this->dataPtr->linkToVisualEntities[link].end()); + } + + // Toggle transparent mode + bool showTransparent, showTransparentInit = false; + + // first loop looks for new transparent entities + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingTransparent.find(visEntity) == + this->dataPtr->viewingTransparent.end()) + { + this->dataPtr->newTransparentEntities.push_back(_entity); + showTransparentInit = showTransparent = true; + } + } + + // second loop toggles transparent mode + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingTransparent.find(visEntity) == + this->dataPtr->viewingTransparent.end()) + continue; + + // when viewing multiple transparent visuals (e.g. _entity is a model), + // boolean for view as transparent is based on first visEntity in list + if (!showTransparentInit) + { + showTransparent = !this->dataPtr->viewingTransparent[visEntity]; + showTransparentInit = true; + } + + rendering::VisualPtr transparentVisual = + this->dataPtr->sceneManager.VisualById(visEntity); + if (transparentVisual == nullptr) + { + ignerr << "Could not find visual for entity [" << visEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingTransparent[visEntity] = showTransparent; + + this->dataPtr->sceneManager.UpdateTransparency(transparentVisual, + showTransparent); + + if (showTransparent) + { + // turn off wireboxes for visual entity + this->HideWireboxes(visEntity); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewWireframes(const Entity &_entity) +{ + std::vector visEntities; + + if (this->dataPtr->linkToVisualEntities.find(_entity) != + this->dataPtr->linkToVisualEntities.end()) + { + visEntities = this->dataPtr->linkToVisualEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_entity)); + + for (const auto &link : links) + { + visEntities.insert(visEntities.end(), + this->dataPtr->linkToVisualEntities[link].begin(), + this->dataPtr->linkToVisualEntities[link].end()); + } + + // Toggle wireframes + bool showWireframe, showWireframeInit = false; + + // first loop looks for new wireframes + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingWireframes.find(visEntity) == + this->dataPtr->viewingWireframes.end()) + { + this->dataPtr->newWireframes.push_back(_entity); + showWireframeInit = showWireframe = true; + } + } + + // second loop toggles wireframes + for (const auto &visEntity : visEntities) + { + if (this->dataPtr->viewingWireframes.find(visEntity) == + this->dataPtr->viewingWireframes.end()) + continue; + + // when viewing multiple wireframes (e.g. _entity is a model), + // boolean for view wireframe is based on first visEntity in list + if (!showWireframeInit) + { + showWireframe = !this->dataPtr->viewingWireframes[visEntity]; + showWireframeInit = true; + } + + rendering::VisualPtr wireframeVisual = + this->dataPtr->sceneManager.VisualById(visEntity); + if (wireframeVisual == nullptr) + { + ignerr << "Could not find visual for entity [" << visEntity + << "]" << std::endl; + continue; + } + + this->dataPtr->viewingWireframes[visEntity] = showWireframe; + wireframeVisual->SetWireframe(showWireframe); + + if (showWireframe) + { + // turn off wireboxes for visual entity + this->HideWireboxes(visEntity); + } + } +} + +///////////////////////////////////////////////// +void RenderUtil::ViewCollisions(const Entity &_entity) +{ + std::vector colEntities; + + if (this->dataPtr->linkToCollisionEntities.find(_entity) != + this->dataPtr->linkToCollisionEntities.end()) + { + colEntities = this->dataPtr->linkToCollisionEntities[_entity]; + } + + // Find all existing child links for this entity + std::vector links = std::move(this->FindChildLinks(_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) { @@ -2306,16 +3570,7 @@ void RenderUtil::ViewCollisions(const Entity &_entity) if (showCol) { - // turn off wireboxes for collision entity - if (this->dataPtr->wireBoxes.find(colEntity) - != this->dataPtr->wireBoxes.end()) - { - ignition::rendering::WireBoxPtr wireBox = - this->dataPtr->wireBoxes[colEntity]; - auto visParent = wireBox->Parent(); - if (visParent) - visParent->SetVisible(false); - } + this->HideWireboxes(colEntity); } } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 133d8d42fd9..613210ce625 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -17,6 +17,8 @@ #include +#include +#include #include #include @@ -41,15 +43,19 @@ #include #include "ignition/rendering/Capsule.hh" +#include #include #include #include +#include +#include #include #include #include #include #include #include +#include #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/Util.hh" @@ -73,27 +79,33 @@ class ignition::gazebo::SceneManagerPrivate public: rendering::ScenePtr scene; /// \brief Map of visual entity in Gazebo to visual pointers. - public: std::map visuals; + public: std::unordered_map visuals; /// \brief Map of actor entity in Gazebo to actor pointers. - public: std::map actors; + public: std::unordered_map actors; /// \brief Map of actor entity in Gazebo to actor animations. - public: std::map actorSkeletons; + public: std::unordered_map actorSkeletons; /// \brief Map of actor entity to the associated trajectories. - public: std::map> + public: std::unordered_map> actorTrajectories; /// \brief Map of light entity in Gazebo to light pointers. - public: std::map lights; + public: std::unordered_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; + public: std::unordered_map sensors; + + /// \brief The map of the original transparency values for the nodes. + public: std::map originalTransparency; + + /// \brief The map of the original depth write values for the nodes. + public: std::map originalDepthWrite; /// \brief Helper function to compute actor trajectory at specified tiime /// \param[in] _id Actor entity's unique id @@ -180,6 +192,8 @@ rendering::VisualPtr SceneManager::CreateModel(Entity _id, } rendering::VisualPtr modelVis = this->dataPtr->scene->CreateVisual(name); + + // \todo(anyone) change to uint64_t once UserData supports this type modelVis->SetUserData("gazebo-entity", static_cast(_id)); modelVis->SetUserData("pause-update", static_cast(0)); modelVis->SetLocalPose(_model.RawPose()); @@ -228,7 +242,6 @@ rendering::VisualPtr SceneManager::CreateLink(Entity _id, linkVis->SetUserData("gazebo-entity", static_cast(_id)); linkVis->SetUserData("pause-update", static_cast(0)); linkVis->SetLocalPose(_link.RawPose()); - linkVis->SetUserData("gazebo-entity", static_cast(_id)); this->dataPtr->visuals[_id] = linkVis; if (parent) @@ -391,6 +404,176 @@ rendering::VisualPtr SceneManager::CreateVisual(Entity _id, return visualVis; } +///////////////////////////////////////////////// +std::vector SceneManager::Filter(const std::string &_node, + std::function _filter) const +{ + std::vector filteredNodes; + + // make sure there is a rendering node named _node + auto rootNode = this->dataPtr->scene->NodeByName(_node); + if (!rootNode) + { + ignerr << "Could not find a node with the name [" << _node + << "] in the scene." << std::endl; + return filteredNodes; + } + + // go through _node and its children in top level order, applying _filter to + // each node + std::queue remainingNodes; + remainingNodes.push(rootNode); + while (!remainingNodes.empty()) + { + auto currentNode = remainingNodes.front(); + remainingNodes.pop(); + if (_filter(currentNode)) + filteredNodes.push_back(currentNode); + + for (auto i = 0u; i < currentNode->ChildCount(); ++i) + remainingNodes.push(currentNode->ChildByIndex(i)); + } + + return filteredNodes; +} + +///////////////////////////////////////////////// +std::pair> SceneManager::CopyVisual( + Entity _id, const std::string &_visual, Entity _parentId) +{ + std::pair> result; + if (!this->dataPtr->scene) + return result; + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return result; + } + + rendering::VisualPtr originalVisual = + std::dynamic_pointer_cast( + this->dataPtr->scene->NodeByName(_visual)); + if (!originalVisual) + { + ignerr << "Could not find a node with the name [" << _visual + << "] in the scene." << std::endl; + return result; + } + + auto name = originalVisual->Name() + "::" + std::to_string(_id); + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + ignerr << "Parent entity with Id: [" << _parentId << "] not found. " + << "Not adding visual with ID [" << _id + << "] and name [" << name << "] to the rendering scene." + << std::endl; + return result; + } + parent = it->second; + } + + if (parent) + name = parent->Name() + "::" + name; + + if (this->dataPtr->scene->HasVisualName(name)) + { + ignerr << "Visual: [" << name << "] already exists" << std::endl; + return result; + } + + // filter visuals that were created by the gui (these shouldn't be cloned) + auto filteredVisuals = this->Filter(_visual, + [](const rendering::NodePtr _node) + { + return _node->HasUserData("gui-only"); + }); + + // temporarily detach filtered visuals, but keep track of the original parent + // so that the visuals can be re-attached later + std::unordered_map + removedVisualToParent; + for (auto filteredVis : filteredVisuals) + { + removedVisualToParent[filteredVis] = filteredVis->Parent(); + filteredVis->RemoveParent(); + } + + // clone the visual + auto clonedVisual = originalVisual->Clone(name, parent); + this->dataPtr->visuals[_id] = clonedVisual; + + // re-attach filtered visuals now that cloning is complete + for (auto &[removedVisual, originalParent] : removedVisualToParent) + originalParent->AddChild(removedVisual); + + // The Clone call above also clones any child visuals that exist, so we need + // to keep track of these new child visuals as well. We get a level order + // listing of all visuals associated with the newly copied visual + bool childrenTracked = true; + std::queue remainingVisuals; + remainingVisuals.push(_id); + std::vector childVisualIds; + while (!remainingVisuals.empty()) + { + const auto topLevelId = remainingVisuals.front(); + remainingVisuals.pop(); + const auto visual = this->dataPtr->visuals[topLevelId]; + for (auto i = 0u; i < visual->ChildCount(); ++i) + { + auto childId = this->UniqueId(); + if (!childId) + { + ignerr << "Unable to create an entity ID for the copied visual's " + << "child, so the copied visual will be deleted.\n"; + childrenTracked = false; + break; + } + auto childVisual = std::dynamic_pointer_cast( + visual->ChildByIndex(i)); + if (!childVisual) + { + ignerr << "Unable to retrieve a child visual of the copied visual, " + << "so the copied visual will be deleted.\n"; + childrenTracked = false; + break; + } + + this->dataPtr->visuals[childId] = childVisual; + childVisual->SetUserData("gazebo-entity", static_cast(childId)); + childVisual->SetUserData("pause-update", static_cast(0)); + childVisualIds.push_back(childId); + + remainingVisuals.push(childId); + } + } + + if (!childrenTracked) + { + this->dataPtr->scene->DestroyVisual(clonedVisual, true); + for (const auto id : childVisualIds) + this->dataPtr->visuals.erase(id); + } + else + { + clonedVisual->SetUserData("gazebo-entity", static_cast(_id)); + clonedVisual->SetUserData("pause-update", static_cast(0)); + + result = {clonedVisual, std::move(childVisualIds)}; + + if (!parent) + this->dataPtr->scene->RootVisual()->AddChild(clonedVisual); + } + + return result; +} + ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::VisualById(Entity _id) { @@ -419,6 +602,7 @@ rendering::VisualPtr SceneManager::CreateCollision(Entity _id, visual.SetName(_collision.Name()); rendering::VisualPtr collisionVis = CreateVisual(_id, visual, _parentId); + collisionVis->SetUserData("gui-only", static_cast(true)); return collisionVis; } ///////////////////////////////////////////////// @@ -519,6 +703,7 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, rendering::HeightmapDescriptor descriptor; descriptor.SetData(data); descriptor.SetSize(_geom.HeightmapShape()->Size()); + descriptor.SetPosition(_geom.HeightmapShape()->Position()); descriptor.SetSampling(_geom.HeightmapShape()->Sampling()); for (uint64_t i = 0; i < _geom.HeightmapShape()->TextureCount(); ++i) @@ -526,10 +711,10 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, auto textureSdf = _geom.HeightmapShape()->TextureByIndex(i); rendering::HeightmapTexture textureDesc; textureDesc.SetSize(textureSdf->Size()); - textureDesc.SetDiffuse(asFullPath(textureSdf->Diffuse(), - _geom.HeightmapShape()->FilePath())); - textureDesc.SetNormal(asFullPath(textureSdf->Normal(), - _geom.HeightmapShape()->FilePath())); + textureDesc.SetDiffuse(common::findFile(asFullPath(textureSdf->Diffuse(), + _geom.HeightmapShape()->FilePath()))); + textureDesc.SetNormal(common::findFile(asFullPath(textureSdf->Normal(), + _geom.HeightmapShape()->FilePath()))); descriptor.AddTexture(textureDesc); } @@ -613,8 +798,21 @@ rendering::MaterialPtr SceneManager::LoadMaterial( } else { - ignerr << "PBR material: currently only metal workflow is supported" - << std::endl; + auto specular = pbr->Workflow(sdf::PbrWorkflowType::SPECULAR); + if (specular) + { + ignerr << "PBR material: currently only metal workflow is supported. " + << "Ignition Gazebo will try to render the material using " + << "metal workflow but without Roughness / Metalness settings." + << std::endl; + } + workflow = const_cast(specular); + } + + if (!workflow) + { + ignerr << "No valid PBR workflow found. " << std::endl; + return rendering::MaterialPtr(); } // albedo map @@ -692,7 +890,7 @@ rendering::MaterialPtr SceneManager::LoadMaterial( ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateActor(Entity _id, - const sdf::Actor &_actor, Entity _parentId) + const sdf::Actor &_actor, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::VisualPtr(); @@ -708,9 +906,6 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, return rendering::VisualPtr(); } - std::string name = _actor.Name().empty() ? std::to_string(_id) : - _actor.Name(); - rendering::VisualPtr parent; if (_parentId != this->dataPtr->worldId) { @@ -718,14 +913,15 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, if (it == this->dataPtr->visuals.end()) { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " - << "Not adding actor with ID[" << _id - << "] and name [" << name << "] to the rendering scene." + << "Not adding actor with ID [" << _id + << "] and name [" << _name << "] to the rendering scene." << std::endl; return rendering::VisualPtr(); } parent = it->second; } + std::string name = _name; if (parent) name = parent->Name() + "::" + name; @@ -751,7 +947,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, } unsigned int numAnims = 0; - std::map mapAnimNameId; + std::unordered_map mapAnimNameId; mapAnimNameId[descriptor.meshName] = numAnims++; // Load all animations @@ -886,7 +1082,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, static_cast(point->Time()*1000))); waypoints[pointTp] = point->Pose(); } - trajInfo.SetWaypoints(waypoints); + trajInfo.SetWaypoints(waypoints, trajSdf->Tension()); // Animations are offset by 1 because index 0 is taken by the mesh name auto animation = _actor.AnimationByIndex(trajInfo.AnimIndex()-1); @@ -998,7 +1194,7 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, ///////////////////////////////////////////////// rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, - const sdf::Light &_light, Entity _parentId) + const sdf::Light &_light, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::VisualPtr(); @@ -1010,9 +1206,6 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, return rendering::VisualPtr(); } - std::string name = _light.Name().empty() ? std::to_string(_id) : - _light.Name(); - rendering::LightPtr lightParent; auto it = this->dataPtr->lights.find(_parentId); if (it != this->dataPtr->lights.end()) @@ -1022,13 +1215,13 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, else { ignerr << "Parent entity with Id: [" << _parentId << "] not found. " - << "Not adding light visual with ID[" << _id - << "] and name [" << name << "] to the rendering scene." + << "Not adding light visual with ID [" << _id + << "] and name [" << _name << "] to the rendering scene." << std::endl; return rendering::VisualPtr(); } - name = lightParent->Name() + "::" + name + "Visual"; + std::string name = lightParent->Name() + "::" + _name + "Visual"; if (this->dataPtr->scene->HasVisualName(name)) { @@ -1068,7 +1261,7 @@ rendering::VisualPtr SceneManager::CreateLightVisual(Entity _id, ///////////////////////////////////////////////// rendering::LightPtr SceneManager::CreateLight(Entity _id, - const sdf::Light &_light, Entity _parentId) + const sdf::Light &_light, const std::string &_name, Entity _parentId) { if (!this->dataPtr->scene) return rendering::LightPtr(); @@ -1093,8 +1286,7 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, parent = it->second; } - std::string name = _light.Name().empty() ? std::to_string(_id) : - _light.Name(); + std::string name = _name; if (parent) name = parent->Name() + "::" + name; @@ -1152,6 +1344,233 @@ rendering::LightPtr SceneManager::CreateLight(Entity _id, return light; } +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateInertiaVisual(Entity _id, + const math::Inertiald &_inertia, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::VisualPtr(); + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return rendering::VisualPtr(); + } + + 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::VisualPtr(); + } + parent = it->second; + } + + std::string name = std::to_string(_id); + if (parent) + name = parent->Name() + "::" + name; + + rendering::InertiaVisualPtr inertiaVisual = + this->dataPtr->scene->CreateInertiaVisual(name); + inertiaVisual->SetInertial(_inertia); + + rendering::VisualPtr inertiaVis = + std::dynamic_pointer_cast(inertiaVisual); + inertiaVis->SetUserData("gazebo-entity", static_cast(_id)); + inertiaVis->SetUserData("pause-update", static_cast(0)); + inertiaVis->SetUserData("gui-only", static_cast(true)); + this->dataPtr->visuals[_id] = inertiaVis; + + if (parent) + { + inertiaVis->RemoveParent(); + parent->AddChild(inertiaVis); + } + return inertiaVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateJointVisual( + Entity _id, const sdf::Joint &_joint, + Entity _childId, Entity _parentId) +{ + if (!this->dataPtr->scene) + { + return rendering::VisualPtr(); + } + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return rendering::VisualPtr(); + } + + rendering::VisualPtr parent; + if (_childId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_childId); + 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::VisualPtr(); + } + parent = it->second; + } + + // Name. + std::string name = _joint.Name().empty() ? std::to_string(_id) : + _joint.Name(); + if (parent) + { + name = parent->Name() + "::" + name; + } + + rendering::JointVisualPtr jointVisual = + this->dataPtr->scene->CreateJointVisual(name); + + switch (_joint.Type()) + { + case sdf::JointType::REVOLUTE: + jointVisual->SetType(rendering::JointVisualType::JVT_REVOLUTE); + break; + case sdf::JointType::REVOLUTE2: + jointVisual->SetType(rendering::JointVisualType::JVT_REVOLUTE2); + break; + case sdf::JointType::PRISMATIC: + jointVisual->SetType(rendering::JointVisualType::JVT_PRISMATIC); + break; + case sdf::JointType::UNIVERSAL: + jointVisual->SetType(rendering::JointVisualType::JVT_UNIVERSAL); + break; + case sdf::JointType::BALL: + jointVisual->SetType(rendering::JointVisualType::JVT_BALL); + break; + case sdf::JointType::SCREW: + jointVisual->SetType(rendering::JointVisualType::JVT_SCREW); + break; + case sdf::JointType::GEARBOX: + jointVisual->SetType(rendering::JointVisualType::JVT_GEARBOX); + break; + case sdf::JointType::FIXED: + jointVisual->SetType(rendering::JointVisualType::JVT_FIXED); + break; + default: + jointVisual->SetType(rendering::JointVisualType::JVT_NONE); + break; + } + + if (parent) + { + jointVisual->RemoveParent(); + parent->AddChild(jointVisual); + } + + if (_joint.Axis(1) && + (_joint.Type() == sdf::JointType::REVOLUTE2 || + _joint.Type() == sdf::JointType::UNIVERSAL + )) + { + auto axis1 = _joint.Axis(0)->Xyz(); + auto axis2 = _joint.Axis(1)->Xyz(); + auto axis1UseParentFrame = _joint.Axis(0)->XyzExpressedIn() == "__model__"; + auto axis2UseParentFrame = _joint.Axis(1)->XyzExpressedIn() == "__model__"; + + jointVisual->SetAxis(axis2, axis2UseParentFrame); + + auto it = this->dataPtr->visuals.find(_parentId); + if (it != this->dataPtr->visuals.end()) + { + auto parentName = it->second->Name(); + jointVisual->SetParentAxis( + axis1, parentName, axis1UseParentFrame); + } + } + else if (_joint.Axis(0) && + (_joint.Type() == sdf::JointType::REVOLUTE || + _joint.Type() == sdf::JointType::PRISMATIC + )) + { + auto axis1 = _joint.Axis(0)->Xyz(); + auto axis1UseParentFrame = _joint.Axis(0)->XyzExpressedIn() == "__model__"; + + jointVisual->SetAxis(axis1, axis1UseParentFrame); + } + else + { + // For fixed joint type, scale joint visual to the joint child link + double childSize = + std::max(0.1, parent->BoundingBox().Size().Length()); + auto scale = ignition::math::Vector3d(childSize * 0.2, + childSize * 0.2, childSize * 0.2); + jointVisual->SetLocalScale(scale); + } + + rendering::VisualPtr jointVis = + std::dynamic_pointer_cast(jointVisual); + jointVis->SetUserData("gazebo-entity", static_cast(_id)); + jointVis->SetUserData("pause-update", static_cast(0)); + jointVis->SetUserData("gui-only", static_cast(true)); + jointVis->SetLocalPose(_joint.RawPose()); + this->dataPtr->visuals[_id] = jointVis; + return jointVis; +} + +///////////////////////////////////////////////// +rendering::VisualPtr SceneManager::CreateCOMVisual(Entity _id, + const math::Inertiald &_inertia, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::VisualPtr(); + + if (this->dataPtr->visuals.find(_id) != this->dataPtr->visuals.end()) + { + ignerr << "Entity with Id: [" << _id << "] already exists in the scene" + << std::endl; + return rendering::VisualPtr(); + } + + 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::VisualPtr(); + } + parent = it->second; + } + + if (!parent) + return rendering::VisualPtr(); + + std::string name = std::to_string(_id); + name = parent->Name() + "::" + name; + + rendering::COMVisualPtr comVisual = + this->dataPtr->scene->CreateCOMVisual(name); + comVisual->RemoveParent(); + parent->AddChild(comVisual); + comVisual->SetInertial(_inertia); + + rendering::VisualPtr comVis = + std::dynamic_pointer_cast(comVisual); + comVis->SetUserData("gazebo-entity", static_cast(_id)); + comVis->SetUserData("pause-update", static_cast(0)); + comVis->SetUserData("gui-only", static_cast(true)); + this->dataPtr->visuals[_id] = comVis; + + return comVis; +} + ///////////////////////////////////////////////// rendering::ParticleEmitterPtr SceneManager::CreateParticleEmitter( Entity _id, const msgs::ParticleEmitter &_emitter, Entity _parentId) @@ -1503,13 +1922,6 @@ AnimationUpdateData SceneManager::ActorAnimationAt( return animData; } -///////////////////////////////////////////////// -std::map SceneManager::ActorMeshAnimationAt( - Entity _id, std::chrono::steady_clock::duration _time) const -{ - return this->ActorSkeletonTransformsAt(_id, _time); -} - ///////////////////////////////////////////////// std::map SceneManager::ActorSkeletonTransformsAt( Entity _id, std::chrono::steady_clock::duration _time) const @@ -1607,6 +2019,19 @@ void SceneManager::RemoveEntity(Entity _id) auto it = this->dataPtr->visuals.find(_id); if (it != this->dataPtr->visuals.end()) { + // Remove visual's original transparency from map + rendering::VisualPtr vis = it->second; + this->dataPtr->originalTransparency.erase(vis->Name()); + // Remove visual's original depth write value from map + this->dataPtr->originalDepthWrite.erase(vis->Name()); + + for (auto g = 0u; g < vis->GeometryCount(); ++g) + { + auto geom = vis->GeometryByIndex(g); + this->dataPtr->originalTransparency.erase(geom->Name()); + this->dataPtr->originalDepthWrite.erase(geom->Name()); + } + this->dataPtr->scene->DestroyVisual(it->second); this->dataPtr->visuals.erase(it); return; @@ -1673,43 +2098,136 @@ rendering::NodePtr SceneManager::TopLevelNode( return node; } -///////////////////////////////////////////////// -Entity SceneManager::EntityFromNode(const rendering::NodePtr &_node) const +//////////////////////////////////////////////// +void SceneManager::UpdateTransparency(const rendering::NodePtr &_node, + bool _makeTransparent) { - // TODO(anyone) On Dome, set entity ID into node with SetUserData - auto visual = std::dynamic_pointer_cast(_node); - if (visual) + if (!_node) + return; + + for (auto n = 0u; n < _node->ChildCount(); ++n) { - auto found = std::find_if(std::begin(this->dataPtr->visuals), - std::end(this->dataPtr->visuals), - [&](const std::pair &_item) + auto child = _node->ChildByIndex(n); + this->UpdateTransparency(child, _makeTransparent); + } + + auto vis = std::dynamic_pointer_cast(_node); + if (nullptr == vis) + return; + + // Visual material + auto visMat = vis->Material(); + if (nullptr != visMat) + { + auto visTransparency = + this->dataPtr->originalTransparency.find(vis->Name()); + auto visDepthWrite = + this->dataPtr->originalDepthWrite.find(vis->Name()); + if (_makeTransparent) { - return _item.second == visual; - }); + if (visTransparency == this->dataPtr->originalTransparency.end()) + { + this->dataPtr->originalTransparency[vis->Name()] = + visMat->Transparency(); + } + visMat->SetTransparency(1.0 - ((1.0 - visMat->Transparency()) * 0.5)); - if (found != this->dataPtr->visuals.end()) + if (visDepthWrite == this->dataPtr->originalDepthWrite.end()) + { + this->dataPtr->originalDepthWrite[vis->Name()] = + visMat->DepthWriteEnabled(); + } + visMat->SetDepthWriteEnabled(false); + } + else { - return found->first; + if (visTransparency != this->dataPtr->originalTransparency.end()) + { + visMat->SetTransparency(visTransparency->second); + } + if (visDepthWrite != this->dataPtr->originalDepthWrite.end()) + { + visMat->SetDepthWriteEnabled(visDepthWrite->second); + } } } - auto light = std::dynamic_pointer_cast(_node); - if (light) + for (auto g = 0u; g < vis->GeometryCount(); ++g) { - auto found = std::find_if(std::begin(this->dataPtr->lights), - std::end(this->dataPtr->lights), - [&](const std::pair &_item) + auto geom = vis->GeometryByIndex(g); + + // Geometry material + auto geomMat = geom->Material(); + if (nullptr == geomMat || visMat == geomMat) + continue; + auto geomTransparency = + this->dataPtr->originalTransparency.find(geom->Name()); + auto geomDepthWrite = + this->dataPtr->originalDepthWrite.find(geom->Name()); + + if (_makeTransparent) { - return _item.second == light; - }); + if (geomTransparency == this->dataPtr->originalTransparency.end()) + { + this->dataPtr->originalTransparency[geom->Name()] = + geomMat->Transparency(); + } + geomMat->SetTransparency(1.0 - ((1.0 - geomMat->Transparency()) * 0.5)); - if (found != this->dataPtr->lights.end()) + if (geomDepthWrite == this->dataPtr->originalDepthWrite.end()) + { + this->dataPtr->originalDepthWrite[geom->Name()] = + geomMat->DepthWriteEnabled(); + } + geomMat->SetDepthWriteEnabled(false); + } + else { - return found->first; + if (geomTransparency != this->dataPtr->originalTransparency.end()) + { + geomMat->SetTransparency(geomTransparency->second); + } + if (geomDepthWrite != this->dataPtr->originalDepthWrite.end()) + { + geomMat->SetDepthWriteEnabled(geomDepthWrite->second); + } } } +} + +//////////////////////////////////////////////// +void SceneManager::UpdateJointParentPose(Entity _jointId) +{ + auto visual = + this->VisualById(_jointId); + + rendering::JointVisualPtr jointVisual = + std::dynamic_pointer_cast(visual); + + auto childPose = jointVisual->WorldPose(); + + if (jointVisual->ParentAxisVisual()) + { + jointVisual->ParentAxisVisual()->SetWorldPose(childPose); + + // scale parent axis visual to the child + auto childScale = jointVisual->LocalScale(); + jointVisual->ParentAxisVisual()->SetLocalScale(childScale); + } +} - return kNullEntity; +///////////////////////////////////////////////// +Entity SceneManager::UniqueId() const +{ + auto id = std::numeric_limits::max(); + while (true) + { + if (!this->HasEntity(id)) + return id; + else if (id == 0u) + return kNullEntity; + --id; + } } ///////////////////////////////////////////////// diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index eac401c9095..0e8b146965f 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -80,8 +80,9 @@ function(gz_add_system system_name) "$" "$/${unversioned}") else() - EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned}) - INSTALL(FILES ${PROJECT_BINARY_DIR}/${unversioned} DESTINATION ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}) + file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") + EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") + INSTALL(FILES ${PROJECT_BINARY_DIR}/lib/${unversioned} DESTINATION ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}) endif() endfunction() @@ -92,12 +93,14 @@ add_subdirectory(apply_joint_force) add_subdirectory(battery_plugin) add_subdirectory(breadcrumbs) add_subdirectory(buoyancy) +add_subdirectory(buoyancy_engine) add_subdirectory(collada_world_exporter) add_subdirectory(contact) add_subdirectory(camera_video_recorder) add_subdirectory(detachable_joint) add_subdirectory(diff_drive) add_subdirectory(follow_actor) +add_subdirectory(force_torque) add_subdirectory(hydrodynamics) add_subdirectory(imu) add_subdirectory(joint_controller) @@ -105,6 +108,7 @@ add_subdirectory(joint_position_controller) add_subdirectory(joint_state_publisher) add_subdirectory(joint_traj_control) add_subdirectory(kinetic_energy_monitor) +add_subdirectory(label) add_subdirectory(lift_drag) add_subdirectory(log) add_subdirectory(log_video_recorder) @@ -127,6 +131,8 @@ add_subdirectory(sensors) add_subdirectory(thermal) add_subdirectory(thruster) add_subdirectory(touch_plugin) +add_subdirectory(track_controller) +add_subdirectory(tracked_vehicle) add_subdirectory(triggered_publisher) add_subdirectory(user_commands) add_subdirectory(velocity_control) diff --git a/src/systems/air_pressure/AirPressure.cc b/src/systems/air_pressure/AirPressure.cc index c3e45a81ec2..10db4bff00c 100644 --- a/src/systems/air_pressure/AirPressure.cc +++ b/src/systems/air_pressure/AirPressure.cc @@ -56,6 +56,20 @@ class ignition::gazebo::systems::AirPressurePrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// True if the rendering component is initialized + public: bool initialized = false; + + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _airPressure AirPressureSensor component. + /// \param[in] _parent Parent entity component. + public: void AddAirPressure( + EntityComponentManager &_ecm, + const Entity _entity, + const components::AirPressureSensor *_airPressure, + const components::ParentEntity *_parent); + /// \brief Create air pressure sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateAirPressureEntities(EntityComponentManager &_ecm); @@ -119,55 +133,80 @@ void AirPressure::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) +void AirPressurePrivate::AddAirPressure( + EntityComponentManager &_ecm, + const Entity _entity, + const components::AirPressureSensor *_airPressure, + const components::ParentEntity *_parent) { - IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities"); - // Create air pressure sensors - _ecm.EachNew( - [&](const Entity &_entity, - const components::AirPressureSensor *_airPressure, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _airPressure->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) - { - std::string topic = scopedName(_entity, _ecm) + "/air_pressure"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::AirPressureSensor>(data); - if (nullptr == sensor) - { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; - return true; - } + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _airPressure->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/air_pressure"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::AirPressureSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); - // The WorldPose component was just created and so it's empty - // We'll compute the world pose manually here - // set sensor world pose - math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); - sensor->SetPose(sensorWorldPose); + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + // set sensor world pose + math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); + sensor->SetPose(sensorWorldPose); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} - return true; - }); +////////////////////////////////////////////////// +void AirPressurePrivate::CreateAirPressureEntities(EntityComponentManager &_ecm) +{ + IGN_PROFILE("AirPressurePrivate::CreateAirPressureEntities"); + if (!this->initialized) + { + // Create air pressure sensors + _ecm.Each( + [&](const Entity &_entity, + const components::AirPressureSensor *_airPressure, + const components::ParentEntity *_parent)->bool + { + AddAirPressure(_ecm, _entity, _airPressure, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create air pressure sensors + _ecm.EachNew( + [&](const Entity &_entity, + const components::AirPressureSensor *_airPressure, + const components::ParentEntity *_parent)->bool + { + AddAirPressure(_ecm, _entity, _airPressure, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/altimeter/Altimeter.cc b/src/systems/altimeter/Altimeter.cc index 65744608a65..ca77a040779 100644 --- a/src/systems/altimeter/Altimeter.cc +++ b/src/systems/altimeter/Altimeter.cc @@ -58,6 +58,20 @@ class ignition::gazebo::systems::AltimeterPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// True if the rendering component is initialized + public: bool initialized = false; + + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _altimeter Altimeter component. + /// \param[in] _parent Parent entity component. + public: void AddAltimeter( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Altimeter *_altimeter, + const components::ParentEntity *_parent); + /// \brief Create altimeter sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateAltimeterEntities(EntityComponentManager &_ecm); @@ -120,56 +134,81 @@ void Altimeter::PostUpdate(const UpdateInfo &_info, } ////////////////////////////////////////////////// -void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) +void AltimeterPrivate::AddAltimeter( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Altimeter *_altimeter, + const components::ParentEntity *_parent) { - IGN_PROFILE("Altimeter::CreateAltimeterEntities"); - // Create altimeters - _ecm.EachNew( - [&](const Entity &_entity, - const components::Altimeter *_altimeter, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _altimeter->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) - { - std::string topic = scopedName(_entity, _ecm) + "/altimeter"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::AltimeterSensor>(data); - if (nullptr == sensor) - { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; - return true; - } + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _altimeter->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/altimeter"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::AltimeterSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); - // Get initial pose of sensor and set the reference z pos - // The WorldPose component was just created and so it's empty - // We'll compute the world pose manually here - double verticalReference = worldPose(_entity, _ecm).Pos().Z(); - sensor->SetVerticalReference(verticalReference); - sensor->SetPosition(verticalReference); + // Get initial pose of sensor and set the reference z pos + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + double verticalReference = worldPose(_entity, _ecm).Pos().Z(); + sensor->SetVerticalReference(verticalReference); + sensor->SetPosition(verticalReference); - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} - return true; - }); +////////////////////////////////////////////////// +void AltimeterPrivate::CreateAltimeterEntities(EntityComponentManager &_ecm) +{ + IGN_PROFILE("Altimeter::CreateAltimeterEntities"); + if (!this->initialized) + { + // Create altimeters + _ecm.Each( + [&](const Entity &_entity, + const components::Altimeter *_altimeter, + const components::ParentEntity *_parent)->bool + { + AddAltimeter(_ecm, _entity, _altimeter, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create altimeters + _ecm.EachNew( + [&](const Entity &_entity, + const components::Altimeter *_altimeter, + const components::ParentEntity *_parent)->bool + { + AddAltimeter(_ecm, _entity, _altimeter, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/buoyancy/Buoyancy.cc b/src/systems/buoyancy/Buoyancy.cc index 81a880a21af..ff91c834718 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -16,8 +16,11 @@ */ #include +#include #include #include +#include +#include #include #include @@ -39,6 +42,7 @@ #include "ignition/gazebo/components/Gravity.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/Volume.hh" #include "ignition/gazebo/components/World.hh" @@ -54,15 +58,34 @@ using namespace systems; class ignition::gazebo::systems::BuoyancyPrivate { - /// \brief Get the fluid density based on a pose. This function can be - /// used to adjust the fluid density based on the pose of an object in the - /// world. This function currently returns a constant value, see the todo - /// in the function implementation. + public: enum BuoyancyType + { + /// \brief Applies same buoyancy to whole world. + UNIFORM_BUOYANCY, + /// \brief Uses z-axis to determine buoyancy of the world + /// This is useful for worlds where we want to simulate the ocean interface. + /// Or for instance if we want to simulate different levels of buoyancies + /// at different depths. + GRADED_BUOYANCY + }; + public: BuoyancyType buoyancyType{BuoyancyType::UNIFORM_BUOYANCY}; + /// \brief Get the fluid density based on a pose. /// \param[in] _pose The pose to use when computing the fluid density. The /// pose frame is left undefined because this function currently returns /// a constant value, see the todo in the function implementation. /// \return The fluid density at the givein pose. - public: double FluidDensity(const math::Pose3d &_pose) const; + public: double UniformFluidDensity(const math::Pose3d &_pose) const; + + /// \brief Get the resultant buoyant force on a shape. + /// \param[in] _pose The pose of the shape. + /// \param[in] _shape The collision mesh of a shape. Currently must + /// be one of box, cylinder or sphere. + /// Updates this->buoyancyForces containing {force, center_of_volume} to be + /// applied on the link. + public: + template + void GradedFluidDensity( + const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity); /// \brief Model interface public: Entity world{kNullEntity}; @@ -70,21 +93,141 @@ class ignition::gazebo::systems::BuoyancyPrivate /// \brief The density of the fluid in which the object is submerged in /// kg/m^3. Defaults to 1000, the fluid density of water. public: double fluidDensity{1000}; + + /// \brief When using GradedBuoyancy, we provide a different buoyancy for + /// each layer. The key on this map is height in meters and the value is fluid + /// density. I.E all the fluid between $key$m and $next_key$m has the density + /// $value$kg/m^3. Everything below the first key is considered as having + /// fluidDensity. + public: std::map layers; + + /// \brief Point from where to apply the force + public: struct BuoyancyActionPoint + { + /// \brief The force to be applied + math::Vector3d force; + + /// \brief The point from which the force will be applied + math::Vector3d point; + + /// \brief The pose of the link in question + math::Pose3d pose; + }; + + /// \brief List of points from where the forces act. + public: std::vector buoyancyForces; + + /// \brief Resolve all forces as if they act as a Wrench from the give pose. + /// \param[in] _pose The point from which all poses are to be resolved. + /// \return A pair of {force, torque} describing the wrench to be applied + /// at _pose. + public: std::pair ResolveForces( + const math::Pose3d &_pose); + + /// \brief Scoped names of entities that buoyancy should apply to. If empty, + /// all links will receive buoyancy. + public: std::unordered_set enabled; }; ////////////////////////////////////////////////// -double BuoyancyPrivate::FluidDensity(const math::Pose3d & /*_pose*/) const +double BuoyancyPrivate::UniformFluidDensity(const math::Pose3d &/*_pose*/) const { - // \todo(nkoenig) Adjust the fluid density based on the provided pose. - // This could take into acount: - // 1. Transition from water to air. Currently this function is used for - // a whole link, but when transitioning between mediums a link may span - // both mediums. Surface tension could also be a factor. - // 2. Fluid density changes based on depth below the water surface and - // height above water surface. return this->fluidDensity; } +////////////////////////////////////////////////// +template +void BuoyancyPrivate::GradedFluidDensity( + const math::Pose3d &_pose, const T &_shape, const math::Vector3d &_gravity) +{ + auto prevLayerFluidDensity = this->fluidDensity; + auto prevLayerVol = 0.0; + auto centerOfBuoyancy = math::Vector3d{0, 0, 0}; + + for (const auto &[height, currFluidDensity] : this->layers) + { + // TODO(arjo): Transform plane and slice the shape + math::Planed plane{math::Vector3d{0, 0, 1}, height - _pose.Pos().Z()}; + auto vol = _shape.VolumeBelow(plane); + + // Short circuit. + if (vol <= 0) + { + prevLayerFluidDensity = currFluidDensity; + continue; + } + + // Calculate point from which force is applied + auto cov = _shape.CenterOfVolumeBelow(plane); + + if (!cov.has_value()) + { + prevLayerFluidDensity = currFluidDensity; + continue; + } + + // Archimedes principle for this layer + auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity; + + // Accumulate layers. + prevLayerFluidDensity = currFluidDensity; + + auto cob = (cov.value() * vol - centerOfBuoyancy * prevLayerVol) + / (vol - prevLayerVol); + centerOfBuoyancy = cov.value(); + auto buoyancyAction = BuoyancyActionPoint + { + forceMag, + cob, + _pose + }; + this->buoyancyForces.push_back(buoyancyAction); + + prevLayerVol = vol; + } + // For the rest of the layers. + auto vol = _shape.Volume(); + + // No force contributed by this layer. + if (std::abs(vol - prevLayerVol) < 1e-10) + return; + + // Archimedes principle for this layer + auto forceMag = - (vol - prevLayerVol) * _gravity * prevLayerFluidDensity; + + // Calculate centre of buoyancy + auto cov = math::Vector3d{0, 0, 0}; + auto cob = + (cov * vol - centerOfBuoyancy * prevLayerVol) / (vol - prevLayerVol); + centerOfBuoyancy = cov; + auto buoyancyAction = BuoyancyActionPoint + { + forceMag, + cob, + _pose + }; + this->buoyancyForces.push_back(buoyancyAction); +} + +////////////////////////////////////////////////// +std::pair BuoyancyPrivate::ResolveForces( + const math::Pose3d &_pose) +{ + auto force = math::Vector3d{0, 0, 0}; + auto torque = math::Vector3d{0, 0, 0}; + + for (const auto &b : this->buoyancyForces) + { + force += b.force; + math::Pose3d localPoint{b.point, math::Quaterniond{1, 0, 0, 0}}; + auto globalPoint = b.pose * localPoint; + auto offset = globalPoint.Pos() - _pose.Pos(); + torque += force.Cross(offset); + } + + return {force, torque}; +} + ////////////////////////////////////////////////// Buoyancy::Buoyancy() : dataPtr(std::make_unique()) @@ -114,6 +257,58 @@ void Buoyancy::Configure(const Entity &_entity, { this->dataPtr->fluidDensity = _sdf->Get("uniform_fluid_density"); } + else if (_sdf->HasElement("graded_buoyancy")) + { + this->dataPtr->buoyancyType = + BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY; + + auto gradedElement = _sdf->GetFirstElement(); + if (gradedElement == nullptr) + { + ignerr << "Unable to get element description" << std::endl; + return; + } + + auto argument = gradedElement->GetFirstElement(); + while (argument != nullptr) + { + if (argument->GetName() == "default_density") + { + argument->GetValue()->Get(this->dataPtr->fluidDensity); + igndbg << "Default density set to " + << this->dataPtr->fluidDensity << std::endl; + } + if (argument->GetName() == "density_change") + { + auto depth = argument->Get("above_depth", 0.0); + auto density = argument->Get("density", 0.0); + if (!depth.second) + { + ignwarn << "No tag was found as a " + << "child of " << std::endl; + } + if (!density.second) + { + ignwarn << "No tag was found as a " + << "child of " << std::endl; + } + this->dataPtr->layers[depth.first] = density.first; + igndbg << "Added layer at " << depth.first << ", " + << density.first << std::endl; + } + argument = argument->GetNextElement(); + } + } + + if (_sdf->HasElement("enable")) + { + for (auto enableElem = _sdf->FindElement("enable"); + enableElem != nullptr; + enableElem = enableElem->GetNextElement("enable")) + { + this->dataPtr->enabled.insert(enableElem->Get()); + } + } } ////////////////////////////////////////////////// @@ -145,14 +340,18 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, return true; } + if (!this->IsEnabled(_entity, _ecm)) + { + return true; + } + Link link(_entity); std::vector collisions = _ecm.ChildrenByComponents( _entity, components::Collision()); double volumeSum = 0; - ignition::math::Vector3d weightedPosSum = - ignition::math::Vector3d::Zero; + math::Vector3d weightedPosSum = math::Vector3d::Zero; // Compute the volume of the link by iterating over all the collision // elements and storing each geometry's volume. @@ -240,32 +439,130 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, const components::Volume *_volume, const components::CenterOfVolume *_centerOfVolume) -> bool { + auto newPose = enableComponent(_ecm, _entity); + newPose |= enableComponent(_ecm, _entity); + if (newPose) + { + // Skip entity if WorldPose and inertial are not yet ready + return true; + } + // World pose of the link. math::Pose3d linkWorldPose = worldPose(_entity, _ecm); + Link link(_entity); + + math::Vector3d buoyancy; // By Archimedes' principle, // buoyancy = -(mass*gravity)*fluid_density/object_density // object_density = mass/volume, so the mass term cancels. - math::Vector3d buoyancy = - -this->dataPtr->FluidDensity(linkWorldPose) * + if (this->dataPtr->buoyancyType + == BuoyancyPrivate::BuoyancyType::UNIFORM_BUOYANCY) + { + buoyancy = + -this->dataPtr->UniformFluidDensity(linkWorldPose) * _volume->Data() * gravity->Data(); - // Convert the center of volume to the world frame - math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector( - _centerOfVolume->Data()); - // Compute the torque that should be applied due to buoyancy and - // the center of volume. - math::Vector3d torque = offsetWorld.Cross(buoyancy); + // Convert the center of volume to the world frame + math::Vector3d offsetWorld = linkWorldPose.Rot().RotateVector( + _centerOfVolume->Data()); + // Compute the torque that should be applied due to buoyancy and + // the center of volume. + math::Vector3d torque = offsetWorld.Cross(buoyancy); + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, buoyancy, torque); + } + else if (this->dataPtr->buoyancyType + == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY) + { + std::vector collisions = _ecm.ChildrenByComponents( + _entity, components::Collision()); + this->dataPtr->buoyancyForces.clear(); + + for (auto e : collisions) + { + const components::CollisionElement *coll = + _ecm.Component(e); + + auto pose = worldPose(e, _ecm); + + if (!coll) + { + ignerr << "Invalid collision pointer. This shouldn't happen\n"; + continue; + } + + switch (coll->Data().Geom()->Type()) + { + case sdf::GeometryType::BOX: + this->dataPtr->GradedFluidDensity( + pose, + coll->Data().Geom()->BoxShape()->Shape(), + gravity->Data()); + break; + case sdf::GeometryType::SPHERE: + this->dataPtr->GradedFluidDensity( + pose, + coll->Data().Geom()->SphereShape()->Shape(), + gravity->Data()); + break; + default: + { + static bool warned{false}; + if (!warned) + { + ignwarn << "Only and collisions are supported " + << "by the graded buoyancy option." << std::endl; + warned = true; + } + break; + } + } + } + } + auto [force, torque] = this->dataPtr->ResolveForces( + link.WorldInertialPose(_ecm).value()); // Apply the wrench to the link. This wrench is applied in the // Physics System. - Link link(_entity); - link.AddWorldWrench(_ecm, buoyancy, torque); - + link.AddWorldWrench(_ecm, force, torque); return true; }); } +////////////////////////////////////////////////// +bool Buoyancy::IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const +{ + // If there's nothing enabled, all entities are enabled + if (this->dataPtr->enabled.empty()) + return true; + + auto entity = _entity; + while (entity != kNullEntity) + { + // Fully scoped name + auto name = scopedName(entity, _ecm, "::", false); + + // Remove world name + name = removeParentScope(name, "::"); + + if (this->dataPtr->enabled.find(name) != this->dataPtr->enabled.end()) + return true; + + // Check parent + auto parentComp = _ecm.Component(entity); + + if (nullptr == parentComp) + return false; + + entity = parentComp->Data(); + } + + return false; +} + IGNITION_ADD_PLUGIN(Buoyancy, ignition::gazebo::System, Buoyancy::ISystemConfigure, diff --git a/src/systems/buoyancy/Buoyancy.hh b/src/systems/buoyancy/Buoyancy.hh index 55d040cdb8e..d3fbeed95d9 100644 --- a/src/systems/buoyancy/Buoyancy.hh +++ b/src/systems/buoyancy/Buoyancy.hh @@ -46,10 +46,28 @@ namespace systems /// ## System Parameters /// /// * `` sets the density of the fluid that surrounds - /// the buoyant object. + /// the buoyant object. [Units: kgm^-3] + /// * `` allows you to define a world where the buoyancy + /// changes with height. An example of such a world could be if we are + /// simulating an open ocean with its surface and under water behaviour. This + /// mode slices the volume of the collision mesh according to where the water + /// line is set. When defining a `` tag, one must also define + /// `` and `` tags. + /// * `` is the default fluid which the world should be + /// filled with. [Units: kgm^-3] + /// * `` allows you to define a new layer. + /// * `` a child property of ``. This determines + /// the height at which the next fluid layer should start. [Units: m] + /// * `` the density of the fluid in this layer. [Units: kgm^-3] + /// * `` used to indicate which models will have buoyancy. + /// Add one enable element per model or link. This element accepts names + /// scoped from the top level model (i.e. `::::`). + /// If there are no enabled entities, all models in simulation will be + /// affected by buoyancy. /// - /// ## Example + /// ## Examples /// + /// ### `uniform_fluid_density` world. /// The `buoyancy.sdf` SDF file contains three submarines. The first /// submarine is neutrally buoyant, the second sinks, and the third /// floats. To run: @@ -57,6 +75,41 @@ namespace systems /// ``` /// ign gazebo -v 4 buoyancy.sdf /// ``` + /// + /// ### `graded_buoyancy` world + /// + /// Often when simulating a maritime environment one may need to simulate both + /// surface and underwater vessels. This means the buoyancy plugin needs to + /// take into account two different fluids. One being water with a density of + /// 1000kgm^-3 and another being air with a very light density of say 1kgm^-3. + /// An example for such a configuration may be found in the + /// `graded_buoyancy.sdf` world. + /// + /// ``` + /// ign gazebo -v 4 graded_buoyancy.sdf + /// ``` + /// + /// You should be able to see a sphere bobbing up and down undergoing simple + /// harmonic motion on the surface of the fluid (this is expected behaviour + /// as the SHM is usually damped by the hydrodynamic forces. See the hydro- + /// dynamics plugin for an example of how to use it). The key part of this is + /// + /// ``` + /// + /// 1000 + /// + /// 0 + /// 1 + /// + /// + /// ``` + /// The default density tag says that by default the world has a fluid density + /// of 1000kgm^-3. This essentially states that by default the world is filled + /// with dihydrogen monoxide (aka water). The `` tag + /// essentially establishes the fact that there is a nother fluid. The + /// `` tag says that above z=0 there is another fluid with a + /// different density. The density of that fluid is defined by the `` + /// tag. We will be simulating air with a fluid density of 1kgm^-3. class Buoyancy : public System, public ISystemConfigure, @@ -79,6 +132,13 @@ namespace systems const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) override; + /// \brief Check if an entity is enabled or not. + /// \param[in] _entity Target entity + /// \param[in] _ecm Entity component manager + /// \return True if buoyancy should be applied. + public: bool IsEnabled(Entity _entity, + const EntityComponentManager &_ecm) const; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.cc b/src/systems/buoyancy_engine/BuoyancyEngine.cc new file mode 100644 index 00000000000..14131f6c2d9 --- /dev/null +++ b/src/systems/buoyancy_engine/BuoyancyEngine.cc @@ -0,0 +1,249 @@ +/* + * 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 "BuoyancyEngine.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::BuoyancyEnginePrivateData +{ + /// \brief Callback for incoming commands + /// \param[in] _volumeSetPoint - ignition message containing the desired + /// volume (in m^3) to fill/drain bladder to. + public: void OnCmdBuoyancyEngine( + const ignition::msgs::Double &_volumeSetPoint); + + /// \brief Current volume of bladder in m^3 + public: double bladderVolume = 3e-5; + + /// \brief Maximum inflation rate in m^3*s^-1 + public: double maxInflationRate = 3e-6; + + /// \brief Set-point for volume, in m^3 + public: double volumeSetPoint = 0.000030; + + /// \brief Minimum volume of bladder in m^3 + public: double minVolume = 0.000030; + + /// \brief Maximum volume of bladder in m^3 + public: double maxVolume = 0.000990; + + /// \brief The link which the bladder is attached to + public: ignition::gazebo::Entity linkEntity{kNullEntity}; + + /// \brief The world entity + public: Entity world{kNullEntity}; + + /// \brief The fluid density in kg*m^-3 + public: double fluidDensity = 1000; + + /// \brief The neutral volume in m^3 + public: double neutralVolume = 0.0003; + + /// \brief Trasport node for control + public: ignition::transport::Node node; + + /// \brief Publishes bladder status + public: ignition::transport::Node::Publisher statusPub; + + /// \brief mutex for protecting bladder volume and set point. + public: std::mutex mtx; +}; + +////////////////////////////////////////////////// +void BuoyancyEnginePrivateData::OnCmdBuoyancyEngine( + const ignition::msgs::Double &_volumeSetpoint) +{ + auto volume = std::max(this->minVolume, _volumeSetpoint.data()); + volume = std::min(volume, this->maxVolume); + + std::lock_guard lock(this->mtx); + this->volumeSetPoint = volume; +} + +////////////////////////////////////////////////// +BuoyancyEnginePlugin::BuoyancyEnginePlugin() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +void BuoyancyEnginePlugin::Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/) +{ + auto model = ignition::gazebo::Model(_entity); + if (!_sdf->HasElement("link_name")) + { + ignerr << "Buoyancy Engine must be attached to some link." << std::endl; + return; + } + + this->dataPtr->linkEntity = + model.LinkByName(_ecm, _sdf->Get("link_name")); + if (this->dataPtr->linkEntity == kNullEntity) + { + ignerr << "Link [" << _sdf->Get("link_name") + << "] was not found in model [" << model.Name(_ecm) << "]" << std::endl; + return; + } + + if (_sdf->HasElement("min_volume")) + { + this->dataPtr->minVolume = _sdf->Get("min_volume"); + } + + if (_sdf->HasElement("max_volume")) + { + this->dataPtr->maxVolume = _sdf->Get("max_volume"); + } + + if (_sdf->HasElement("fluid_density")) + { + this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); + } + + this->dataPtr->bladderVolume = this->dataPtr->minVolume; + if (_sdf->HasElement("default_volume")) + { + this->dataPtr->bladderVolume = _sdf->Get("default_volume"); + this->dataPtr->volumeSetPoint = this->dataPtr->bladderVolume; + } + + if (_sdf->HasElement("neutral_volume")) + { + this->dataPtr->neutralVolume = _sdf->Get("neutral_volume"); + } + + if(_sdf->HasElement("max_inflation_rate")) + { + this->dataPtr->maxInflationRate = _sdf->Get("max_inflation_rate"); + } + + this->dataPtr->world = _ecm.EntityByComponents(components::World()); + if (this->dataPtr->world == kNullEntity) + { + ignerr << "World entity not found" <HasElement("namespace")) + { + cmdTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + _sdf->Get("namespace") + "/buoyancy_engine/"); + statusTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + _sdf->Get("namespace") + + "/buoyancy_engine/current_volume"); + } + + if(!this->dataPtr->node.Subscribe(cmdTopic, + &BuoyancyEnginePrivateData::OnCmdBuoyancyEngine, this->dataPtr.get())) + { + ignerr << "Failed to subscribe to [" << cmdTopic << "]" << std::endl; + } + + this->dataPtr->statusPub = + this->dataPtr->node.Advertise(statusTopic); + + igndbg << "Listening to commands on [" << cmdTopic + << "], publishing status on [" << statusTopic << "]" <> DurationInSecs; + auto dt = std::chrono::duration_cast(_info.dt).count(); + + ignition::msgs::Double msg; + + const components::Gravity *gravity = _ecm.Component( + this->dataPtr->world); + if (!gravity) + { + ignerr << "World has no gravity component" << std::endl; + return; + } + + math::Vector3d zForce; + { + std::lock_guard lock(this->dataPtr->mtx); + // Adjust the bladder volume using the pump. Assume ability to pump at + // max flow rate + if (this->dataPtr->bladderVolume < this->dataPtr->volumeSetPoint) + { + this->dataPtr->bladderVolume += + std::min( + dt * this->dataPtr->maxInflationRate, + this->dataPtr->volumeSetPoint - this->dataPtr->bladderVolume + ); + } + else if (this->dataPtr->bladderVolume > this->dataPtr->volumeSetPoint) + { + this->dataPtr->bladderVolume -= + std::min( + dt * this->dataPtr->maxInflationRate, + this->dataPtr->bladderVolume - this->dataPtr->volumeSetPoint + ); + } + + /// Populate status message + msg.set_data(this->dataPtr->bladderVolume); + this->dataPtr->statusPub.Publish(msg); + + // Simply use Archimede's principle to apply a force at the desired link + // position. We take off the neutral buoyancy element in order to simulate + // the mass of the oil in the bladder. + zForce = - gravity->Data() * this->dataPtr->fluidDensity + * (this->dataPtr->bladderVolume - this->dataPtr->neutralVolume); + } + ignition::gazebo::Link link(this->dataPtr->linkEntity); + link.AddWorldWrench(_ecm, zForce, {0, 0, 0}); +} + +IGNITION_ADD_PLUGIN( + BuoyancyEnginePlugin, + ignition::gazebo::System, + BuoyancyEnginePlugin::ISystemConfigure, + BuoyancyEnginePlugin::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(BuoyancyEnginePlugin, + "ignition::gazebo::systems::BuoyancyEngine") diff --git a/src/systems/buoyancy_engine/BuoyancyEngine.hh b/src/systems/buoyancy_engine/BuoyancyEngine.hh new file mode 100644 index 00000000000..c4dd70cf809 --- /dev/null +++ b/src/systems/buoyancy_engine/BuoyancyEngine.hh @@ -0,0 +1,114 @@ +/* + * 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_BUOYANCYENGINE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_BUOYANCYENGINE_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + class BuoyancyEnginePrivateData; + + /// \brief This class provides a simple mechanical bladder which is used to + /// control the buoyancy of an underwater glider. It uses Archimedes' + /// principle to apply an upward force based on the volume of the bladder. It + /// listens to the topic `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine` topic for the volume of the bladder + /// in *cubicmeters*. + /// + /// ## Parameters + /// - The link which the plugin is attached to [required, string] + /// - The namespace for the topic. If empty the plugin will listen + /// on `buoyancy_engine` otherwise it listens on + /// `/model/{namespace}/buoyancy_engine` [optional, string] + /// - Minimum volume of the engine [optional, float, + /// default=0.00003m^3] + /// - At this volume the engine has neutral buoyancy. Used to + /// estimate the weight of the engine [optional, float, default=0.0003m^3] + /// - The volume which the engine starts at [optional, float, + /// default=0.0003m^3] + /// - Maximum volume of the engine [optional, float, + /// default=0.00099m^3] + /// - Maximum inflation rate for bladder [optional, + /// float, default=0.000003m^3/s] + /// - The fluid density of the liquid its suspended in kgm^-3. + /// [optional, float, default=1000kgm^-3] + /// + /// ## Topics + /// * Subscribes to a ignition::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine`. This is the set point for the + /// engine. + /// * Publishes a ignition::msgs::Double on `buoyancy_engine` or + /// `/model/{namespace}/buoyancy_engine/current_volume` on the current volume + /// + /// ## Examples + /// To get started run: + /// ``` + /// ign gazebo buoyancy_engine.sdf + /// ``` + /// Enter the following in a separate terminal: + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double + /// -p "data: 0.003" + /// ``` + /// To see the box float up. + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/ -m ignition.msgs.Double + /// -p "data: 0.001" + /// ``` + /// To see the box go down. + /// To see the current volume enter: + /// ``` + /// ign topic -t /model/buoyant_box/buoyancy_engine/current_volume -e + /// ``` + class BuoyancyEnginePlugin: + public ignition::gazebo::System, + public ignition::gazebo::ISystemConfigure, + public ignition::gazebo::ISystemPreUpdate + { + /// \brief Constructor + public: BuoyancyEnginePlugin(); + + // Documentation inherited + public: void Configure( + const ignition::gazebo::Entity &_entity, + const std::shared_ptr &_sdf, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/ + ); + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; +} +} +} +} +#endif diff --git a/src/systems/buoyancy_engine/CMakeLists.txt b/src/systems/buoyancy_engine/CMakeLists.txt new file mode 100644 index 00000000000..b849e893096 --- /dev/null +++ b/src/systems/buoyancy_engine/CMakeLists.txt @@ -0,0 +1,7 @@ +gz_add_system(buoyancy-engine + SOURCES + BuoyancyEngine.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/camera_video_recorder/CMakeLists.txt b/src/systems/camera_video_recorder/CMakeLists.txt index 0518888b9c0..1d07b852d94 100644 --- a/src/systems/camera_video_recorder/CMakeLists.txt +++ b/src/systems/camera_video_recorder/CMakeLists.txt @@ -4,4 +4,5 @@ gz_add_system(camera-video-recorder PUBLIC_LINK_LIBS ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} + ignition-gazebo${PROJECT_VERSION_MAJOR}-rendering ) diff --git a/src/systems/camera_video_recorder/CameraVideoRecorder.cc b/src/systems/camera_video_recorder/CameraVideoRecorder.cc index 521d69807f4..69180362995 100644 --- a/src/systems/camera_video_recorder/CameraVideoRecorder.cc +++ b/src/systems/camera_video_recorder/CameraVideoRecorder.cc @@ -30,7 +30,9 @@ #include #include +#include "ignition/gazebo/rendering/RenderUtil.hh" #include "ignition/gazebo/rendering/Events.hh" +#include "ignition/gazebo/rendering/MarkerManager.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/Model.hh" @@ -122,11 +124,19 @@ class ignition::gazebo::systems::CameraVideoRecorderPrivate /// By default (false), video encoding is done using real time. public: bool recordVideoUseSimTime = false; - /// \brief Video recorder bitrate (bps) + /// \brief Video recorder bitrate (bps). This is rougly 2Mbps which + /// produces decent video quality while not generating overly large + /// video files. + /// + /// Another point of reference is at: + /// https://support.google.com/youtube/answer/1722171?hl=en#zippy=%2Cbitrate public: unsigned int recordVideoBitrate = 2070000; /// \brief Recording frames per second. public: unsigned int fps = 25; + + /// \brief Marker manager + public: MarkerManager markerManager; }; ////////////////////////////////////////////////// @@ -269,6 +279,8 @@ void CameraVideoRecorderPrivate::OnPostRender() if (!this->scene) { this->scene = rendering::sceneFromFirstRenderEngine(); + this->markerManager.SetTopic(this->sensorTopic + "/marker"); + this->markerManager.Init(this->scene); } // return if scene not ready or no sensors available. @@ -301,6 +313,9 @@ void CameraVideoRecorderPrivate::OnPostRender() std::lock_guard lock(this->updateMutex); + this->markerManager.SetSimTime(this->simTime); + this->markerManager.Update(); + // record video if (this->recordVideo) { @@ -380,18 +395,31 @@ void CameraVideoRecorderPrivate::OnPostRender() // stop encoding this->videoEncoder.Stop(); - // move the tmp video file to user specified path + ignmsg << "Stop video recording on [" << this->service << "]." << std::endl; + if (common::exists(this->tmpVideoFilename)) { - common::moveFile(this->tmpVideoFilename, - this->recordVideoSavePath); + std::string parentPath = common::parentPath(this->recordVideoSavePath); + + // move the tmp video file to user specified path + if (parentPath != this->recordVideoSavePath && + !common::exists(parentPath) && !common::createDirectory(parentPath)) + { + ignerr << "Unable to create directory[" << parentPath + << "]. Video file[" << this->tmpVideoFilename + << "] will not be moved." << std::endl; + } + else + { + common::moveFile(this->tmpVideoFilename, this->recordVideoSavePath); - // Remove old temp file, if it exists. - std::remove(this->tmpVideoFilename.c_str()); + // Remove old temp file, if it exists. + std::remove(this->tmpVideoFilename.c_str()); + + ignmsg << "Saving tmp video[" << this->tmpVideoFilename << "] file to [" + << this->recordVideoSavePath << "]" << std::endl; + } } - ignmsg << "Stop video recording on [" << this->service << "]. " - << "Saving file to: [" << this->recordVideoSavePath << "]" - << std::endl; // reset the event connection to prevent unnecessary render callbacks this->postRenderConn.reset(); diff --git a/src/systems/force_torque/CMakeLists.txt b/src/systems/force_torque/CMakeLists.txt new file mode 100644 index 00000000000..884bb75a639 --- /dev/null +++ b/src/systems/force_torque/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(forcetorque + SOURCES + ForceTorque.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + PRIVATE_LINK_LIBS + ignition-sensors${IGN_SENSORS_VER}::force_torque +) \ No newline at end of file diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc new file mode 100644 index 00000000000..e1668fbf71d --- /dev/null +++ b/src/systems/force_torque/ForceTorque.cc @@ -0,0 +1,352 @@ +/* + * 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 "ForceTorque.hh" + +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/ForceTorque.hh" +#include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/ParentLinkName.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/World.hh" +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Private ForceTorque data class. +class ignition::gazebo::systems::ForceTorquePrivate +{ + /// \brief A map of FT entity to its FT sensor. + public: std::unordered_map> entitySensorMap; + + /// \brief A struct to hold the joint and link entities associated with a + /// sensor + public: struct SensorJointAndLinks + { + /// \brief The parent joint of the sensor + Entity joint; + /// \breif The parent link of the joint + Entity jointParentLink; + /// \breif The child link of the joint + Entity jointChildLink; + }; + + /// \brief Cache of the entities associated with the sensor + public: std::unordered_map sensorJointLinkMap; + + /// \brief Ign-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Get the link entity identified by the given scoped name + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _name Scoped name of the link + /// \param[in] _parentModel The model entity in which the scope of the given + /// name starts. + /// \return The link entity if found, otherwise kNullEntity + public: Entity GetLinkFromScopedName(const EntityComponentManager &_ecm, + const std::string &_name, + Entity _parentModel) const; + /// \brief Create FT sensor + /// \param[in] _ecm Mutable reference to ECM. + public: void CreateForceTorqueEntities(EntityComponentManager &_ecm); + + /// \brief Update FT sensor data based on physics data + /// \param[in] _ecm Immutable reference to ECM. + public: void Update(const EntityComponentManager &_ecm); + + /// \brief Remove FT sensors if their entities have been removed from + /// simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveForceTorqueEntities(const EntityComponentManager &_ecm); +}; + +////////////////////////////////////////////////// +ForceTorque::ForceTorque() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +ForceTorque::~ForceTorque() = default; + +////////////////////////////////////////////////// +void ForceTorque::PreUpdate(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + IGN_PROFILE("ForceTorque::PreUpdate"); + this->dataPtr->CreateForceTorqueEntities(_ecm); +} + +////////////////////////////////////////////////// +void ForceTorque::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("ForceTorque::PostUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + // Only update and publish if not paused. + if (!_info.paused) + { + this->dataPtr->Update(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + it.second.get()->sensors::Sensor::Update(_info.simTime, false); + } + } + + this->dataPtr->RemoveForceTorqueEntities(_ecm); +} + +////////////////////////////////////////////////// +Entity ForceTorquePrivate::GetLinkFromScopedName( + const EntityComponentManager &_ecm, const std::string &_name, + Entity _parentModel) const +{ + auto entities = entitiesFromScopedName(_name, _ecm, _parentModel); + for (const auto & entity : entities) + { + if (_ecm.EntityHasComponentType(entity, components::Link::typeId)) + { + return entity; + } + } + return kNullEntity; +} + +////////////////////////////////////////////////// +void ForceTorquePrivate::CreateForceTorqueEntities(EntityComponentManager &_ecm) +{ + // Create FT Sensors + _ecm.EachNew( + [&](const Entity &_entity, + const components::ForceTorque *_ft)->bool + { + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _ft->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/forcetorque"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::ForceTorqueSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return true; + } + + auto jointEntity = + _ecm.Component(_entity)->Data(); + const std::string jointName = + _ecm.Component(jointEntity)->Data(); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + // Parent has to be a joint + if (!_ecm.EntityHasComponentType(jointEntity, + components::Joint::typeId)) + { + ignerr << "Parent entity of sensor [" << sensorScopedName + << "] must be a joint. Failed to create sensor." << std::endl; + return true; + } + _ecm.CreateComponent(jointEntity, components::JointTransmittedWrench()); + + const auto modelEntity = + _ecm.Component(jointEntity)->Data(); + + // Find the joint parent and child links + const auto jointParentName = + _ecm.Component(jointEntity)->Data(); + auto jointParentLinkEntity = + this->GetLinkFromScopedName(_ecm, jointParentName, modelEntity); + if (kNullEntity == jointParentLinkEntity ) + { + ignerr << "Parent link with name [" << jointParentName + << "] of joint with name [" << jointName + << "] not found. Failed to create sensor [" << sensorScopedName + << "]" << std::endl; + return true; + } + + const auto jointChildName = + _ecm.Component(jointEntity)->Data(); + auto jointChildLinkEntity = + this->GetLinkFromScopedName(_ecm, jointChildName, modelEntity); + if (kNullEntity == jointChildLinkEntity) + { + ignerr << "Child link with name [" << jointChildName + << "] of joint with name [" << jointName + << "] not found. Failed to create sensor [" << sensorScopedName + << "]" << std::endl; + return true; + } + + SensorJointAndLinks sensorJointLinkEntry; + sensorJointLinkEntry.joint = jointEntity; + sensorJointLinkEntry.jointParentLink = jointParentLinkEntity; + sensorJointLinkEntry.jointChildLink = jointChildLinkEntity; + this->sensorJointLinkMap[_entity] = sensorJointLinkEntry; + + auto sensorIt = this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))).first; + + const auto X_WC = worldPose(jointChildLinkEntity, _ecm); + const auto X_CJ = _ecm.Component(jointEntity)->Data(); + const auto X_WJ = X_WC * X_CJ; + const auto X_JS = _ecm.Component(_entity)->Data(); + const auto X_WS = X_WJ * X_JS; + const auto X_SC = X_WS.Inverse() * X_WC; + sensorIt->second->SetRotationChildInSensor(X_SC.Rot()); + return true; + }); +} + +////////////////////////////////////////////////// +void ForceTorquePrivate::Update(const EntityComponentManager &_ecm) +{ + IGN_PROFILE("ForceTorquePrivate::Update"); + _ecm.Each( + [&](const Entity &_entity, const components::ForceTorque *) -> bool + { + auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) + { + auto jointLinkIt = this->sensorJointLinkMap.find(_entity); + if (jointLinkIt == this->sensorJointLinkMap.end()) + { + ignerr << "Failed to update Force/Torque Sensor: " << _entity + << ". Associated entities not found." << std::endl; + return true; + } + + auto jointWrench = _ecm.Component( + jointLinkIt->second.joint); + + // Notation: + // X_WJ: Pose of joint in world + // X_WP: Pose of parent link in world + // X_WC: Pose of child link in world + // X_WS: Pose of sensor in world + // X_SP: Pose of parent link in sensors frame + // X_SC: Pose of child link in sensors frame + const auto X_WP = + worldPose(jointLinkIt->second.jointParentLink, _ecm); + const auto X_WC = worldPose(jointLinkIt->second.jointChildLink, _ecm); + // There appears to be a bug worldPose for computing poses of //joint + // and its children, so we do it manually here. + const auto X_CJ = + _ecm.Component(jointLinkIt->second.joint) + ->Data(); + auto X_WJ = X_WC * X_CJ; + + auto X_JS = _ecm.Component(_entity)->Data(); + auto X_WS = X_WJ * X_JS; + auto X_SP = X_WS.Inverse() * X_WP; + + // The joint wrench is computed at the joint frame. We need to + // transform it the sensor frame. + math::Vector3d force = + X_JS.Rot().Inverse() * msgs::Convert(jointWrench->Data().force()); + + math::Vector3d torque = + X_JS.Rot().Inverse() * + msgs::Convert(jointWrench->Data().torque()) - + X_JS.Pos().Cross(force); + + it->second->SetForce(force); + it->second->SetTorque(torque); + it->second->SetRotationParentInSensor(X_SP.Rot()); + } + else + { + ignerr << "Failed to update Force/Torque Sensor: " << _entity << ". " + << "Entity not found." << std::endl; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void ForceTorquePrivate::RemoveForceTorqueEntities( + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("ForceTorquePrivate::RemoveForceTorqueEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::ForceTorque *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + ignerr << "Internal error, missing FT sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +IGNITION_ADD_PLUGIN(ForceTorque, System, + ForceTorque::ISystemPreUpdate, + ForceTorque::ISystemPostUpdate +) + +IGNITION_ADD_PLUGIN_ALIAS(ForceTorque, "ignition::gazebo::systems::ForceTorque") diff --git a/src/systems/force_torque/ForceTorque.hh b/src/systems/force_torque/ForceTorque.hh new file mode 100644 index 00000000000..5df5652fb0d --- /dev/null +++ b/src/systems/force_torque/ForceTorque.hh @@ -0,0 +1,68 @@ +/* + * 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_FORCE_TORQUE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_FORCE_TORQUE_HH_ + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class ForceTorquePrivate; + + /// \class ForceTorque ForceTorque.hh ignition/gazebo/systems/ForceTorque.hh + /// \brief This system manages all Force-Torque sensors in simulation. + /// Each FT sensor reports readings over Ignition Transport. + /// \note Regardless of the setting of //sensor/force_torque/frame the point + /// of application of the force is at the sensor's origin. + /// //sensor/force_torque/frame only changes the coordinate frame in which the + /// quantites are expressed, not the point of application. + class ForceTorque: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: ForceTorque(); + + /// \brief Destructor + public: ~ForceTorque() override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 6a645a13ec1..5e721fb3288 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -27,8 +27,6 @@ #include -#include - #include #include @@ -58,8 +56,14 @@ class ignition::gazebo::systems::ImuPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// \brief Keep track of world ID, which is equivalent to the scene's + /// root visual. + /// Defaults to zero, which is considered invalid by Ignition Gazebo. public: Entity worldEntity = kNullEntity; + /// True if the rendering component is initialized + public: bool initialized = false; + /// \brief Create IMU sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateImuEntities(EntityComponentManager &_ecm); @@ -68,6 +72,17 @@ class ignition::gazebo::systems::ImuPrivate /// \param[in] _ecm Immutable reference to ECM. public: void Update(const EntityComponentManager &_ecm); + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _imu IMU component. + /// \param[in] _parent Parent entity component. + public: void addIMU( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Imu *_imu, + const components::ParentEntity *_parent); + /// \brief Remove IMU sensors if their entities have been removed from /// simulation. /// \param[in] _ecm Immutable reference to ECM. @@ -121,6 +136,70 @@ void Imu::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveImuEntities(_ecm); } +////////////////////////////////////////////////// +void ImuPrivate::addIMU( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Imu *_imu, + const components::ParentEntity *_parent) +{ + // Get the world acceleration (defined in world frame) + auto gravity = _ecm.Component(worldEntity); + if (nullptr == gravity) + { + ignerr << "World missing gravity." << std::endl; + return; + } + + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _imu->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/imu"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::ImuSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set gravity - assume it remains fixed + sensor->SetGravity(gravity->Data()); + + // Get initial pose of sensor and set the reference z pos + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + math::Pose3d p = worldPose(_entity, _ecm); + sensor->SetOrientationReference(p.Rot()); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + // Set whether orientation is enabled + if (data.ImuSensor()) + { + sensor->SetOrientationEnabled( + data.ImuSensor()->OrientationEnabled()); + } + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + ////////////////////////////////////////////////// void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) { @@ -134,63 +213,31 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) return; } - // Get the world acceleration (defined in world frame) - auto gravity = _ecm.Component(worldEntity); - if (nullptr == gravity) + if (!this->initialized) { - ignerr << "World missing gravity." << std::endl; - return; - } - - // Create IMUs - _ecm.EachNew( - [&](const Entity &_entity, - const components::Imu *_imu, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _imu->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) + // Create IMUs + _ecm.Each( + [&](const Entity &_entity, + const components::Imu *_imu, + const components::ParentEntity *_parent)->bool { - std::string topic = scopedName(_entity, _ecm) + "/imu"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::ImuSensor>(data); - if (nullptr == sensor) + addIMU(_ecm, _entity, _imu, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create IMUs + _ecm.EachNew( + [&](const Entity &_entity, + const components::Imu *_imu, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + addIMU(_ecm, _entity, _imu, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set gravity - assume it remains fixed - sensor->SetGravity(gravity->Data()); - - // Get initial pose of sensor and set the reference z pos - // The WorldPose component was just created and so it's empty - // We'll compute the world pose manually here - math::Pose3d p = worldPose(_entity, _ecm); - sensor->SetOrientationReference(p.Rot()); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 5c14e0f082f..a8a507fd9db 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -20,6 +20,7 @@ #include #include +#include #include #include @@ -27,6 +28,7 @@ #include #include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" #include "ignition/gazebo/components/JointPosition.hh" #include "ignition/gazebo/Model.hh" @@ -63,6 +65,19 @@ class ignition::gazebo::systems::JointPositionControllerPrivate /// \brief Joint index to be used. public: unsigned int jointIndex = 0u; + + /// \brief Operation modes + enum OperationMode + { + /// \brief Use PID to achieve positional control + PID, + /// \brief Bypass PID completely. This means the joint will move to that + /// position bypassing the physics engine. + ABS + }; + + /// \brief Joint position mode + public: OperationMode mode = OperationMode::PID; }; ////////////////////////////////////////////////// @@ -143,6 +158,15 @@ void JointPositionController::Configure(const Entity &_entity, { cmdOffset = _sdf->Get("cmd_offset"); } + if (_sdf->HasElement("use_velocity_commands")) + { + auto useVelocityCommands = _sdf->Get("use_velocity_commands"); + if (useVelocityCommands) + { + this->dataPtr->mode = + JointPositionControllerPrivate::OperationMode::ABS; + } + } this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset); @@ -221,26 +245,29 @@ void JointPositionController::PreUpdate( _ecm.CreateComponent( this->dataPtr->jointEntity, components::JointPosition()); } - if (jointPosComp == nullptr) + // We just created the joint position component, give one iteration for the + // physics system to update its size + if (jointPosComp == nullptr || jointPosComp->Data().empty()) return; // Sanity check: Make sure that the joint index is valid. if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { - static bool invalidJointReported = false; - if (!invalidJointReported) + static std::unordered_set reported; + if (reported.find(this->dataPtr->jointEntity) == reported.end()) { ignerr << "[JointPositionController]: Detected an invalid " << "parameter. The index specified is [" - << this->dataPtr->jointIndex << "] but the joint only has [" + << this->dataPtr->jointIndex << "] but joint [" + << this->dataPtr->jointName << "] only has [" << jointPosComp->Data().size() << "] index[es]. " << "This controller will be ignored" << std::endl; - invalidJointReported = true; + reported.insert(this->dataPtr->jointEntity); } return; } - // Update force command. + // Get error in position double error; { std::lock_guard lock(this->dataPtr->jointCmdMutex); @@ -248,6 +275,48 @@ void JointPositionController::PreUpdate( this->dataPtr->jointPosCmd; } + // Check if the mode is ABS + if (this->dataPtr->mode == + JointPositionControllerPrivate::OperationMode::ABS) + { + // Calculate target velcity + double targetVel = 0; + + // Get time in seconds + auto dt = std::chrono::duration(_info.dt).count(); + + // Get the maximum amount in m that this joint may move + auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; + + // Limit the maximum change to maxMovement + if (abs(error) > maxMovement) + { + targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : + -this->dataPtr->posPid.CmdMax(); + } + else + { + targetVel = -error; + } + + // Set velocity and return + auto vel = + _ecm.Component(this->dataPtr->jointEntity); + + if (vel == nullptr) + { + _ecm.CreateComponent( + this->dataPtr->jointEntity, + components::JointVelocityCmd({targetVel})); + } + else if (!vel->Data().empty()) + { + vel->Data()[0] = targetVel; + } + return; + } + + // Update force command. double force = this->dataPtr->posPid.Update(error, _info.dt); auto forceComp = diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index f1bd6765ed3..1723d689e4d 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -71,6 +71,10 @@ namespace systems /// /// `` Command offset (feed-forward) of the PID. Optional /// parameter. The default value is 0. + /// + /// `` Bypasses the PID and creates a perfect + /// position. The maximum speed on the joint can be set using the `` + /// tag. class JointPositionController : public System, public ISystemConfigure, diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 9830edfbd70..f19b1b155e9 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.cc +++ b/src/systems/joint_state_publisher/JointStatePublisher.cc @@ -31,6 +31,7 @@ #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Util.hh" using namespace ignition; using namespace gazebo; @@ -89,6 +90,14 @@ void JointStatePublisher::Configure( this->CreateComponents(_ecm, joint); } } + + // Advertise the state topic + // Sets to provided topic if available + if (_sdf->HasElement("topic")) + { + this->topic = _sdf->Get("topic"); + } + } ////////////////////////////////////////////////// @@ -142,11 +151,26 @@ void JointStatePublisher::PostUpdate(const UpdateInfo &_info, worldName = _ecm.Component( parentEntity->Data())->Data(); - // Advertise the state topic - std::string topic = std::string("/world/") + worldName + "/model/" - + this->model.Name(_ecm) + "/joint_state"; + // if topic not set it will be empty + std::vector topics; + // this helps avoid unecesarry invalid topic error + if (!this->topic.empty()) + { + topics.push_back(this->topic); + } + topics.push_back(std::string("/world/") + worldName + "/model/" + + this->model.Name(_ecm) + "/joint_state"); + + this->topic = validTopic(topics); + if (this->topic.empty()) + { + ignerr << "No valid topics for JointStatePublisher could be found." + << "Make sure World/Model name does'nt contain invalid characters.\n"; + return; + } + this->modelPub = std::make_unique( - this->node.Advertise(topic)); + this->node.Advertise(this->topic)); } } diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index 352137edaee..75c13f15a74 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include #include @@ -82,6 +83,9 @@ namespace systems /// \brief The joints that will be published. private: std::set joints; + + /// \brief The topic + private: std::string topic; }; } } diff --git a/src/systems/label/CMakeLists.txt b/src/systems/label/CMakeLists.txt new file mode 100644 index 00000000000..22b66ff1cd3 --- /dev/null +++ b/src/systems/label/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_system(label + SOURCES + Label.cc +) diff --git a/src/systems/label/Label.cc b/src/systems/label/Label.cc new file mode 100644 index 00000000000..09986de1048 --- /dev/null +++ b/src/systems/label/Label.cc @@ -0,0 +1,106 @@ +/* + * 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 "Label.hh" + +#include +#include + +#include + +#include "ignition/gazebo/EntityComponentManager.hh" +#include "ignition/gazebo/components/Actor.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/SemanticLabel.hh" +#include "ignition/gazebo/components/Visual.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +////////////////////////////////////////////////// +Label::Label() : System() +{ +} + +////////////////////////////////////////////////// +Label::~Label() = default; + +////////////////////////////////////////////////// +void Label::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager &_ecm, + gazebo::EventManager & /*_eventMgr*/) +{ + const std::string labelTag = "label"; + + if (!_sdf->HasElement(labelTag)) + { + ignerr << "Failed to load Label system; label tag not found.\n"; + return; + } + + auto label = _sdf->Get(labelTag); + + if (label < 0 || label > 255) + { + ignerr << "Failed to configure Label system; value " << label + << " is not in [0-255] range.\n"; + return; + } + + // Attach a semantic label component to the visual. + // If the plugin is inside the tag, get its visual child. + if (_ecm.EntityHasComponentType(_entity, components::Visual::typeId) || + _ecm.EntityHasComponentType(_entity, components::Actor::typeId)) + { + _ecm.CreateComponent(_entity, components::SemanticLabel(label)); + } + else if (_ecm.EntityHasComponentType(_entity, components::Model::typeId)) + { + // TODO(anyone) add support for nested models. We will need to check for + // child models and their respective links/visuals + // https://github.com/ignitionrobotics/ign-gazebo/issues/1041 + + // Get link childern of parent model + auto links = _ecm.ChildrenByComponents( + _entity, components::Link()); + + for (auto linkEntity : links) + { + // get visual child of parent link + auto visuals = _ecm.ChildrenByComponents( + linkEntity, components::Visual()); + + // Create label component to all visual childern + for (auto visualEntity : visuals) + { + _ecm.CreateComponent(visualEntity, + components::SemanticLabel(label)); + } + } + } + else + { + ignerr << "Entity [" << _entity << "] is not a visual, actor, or model. " + << "Label will be ignored. \n"; + return; + } +} + +IGNITION_ADD_PLUGIN(Label, System, Label::ISystemConfigure) +IGNITION_ADD_PLUGIN_ALIAS(Label, "ignition::gazebo::systems::Label") diff --git a/src/systems/label/Label.hh b/src/systems/label/Label.hh new file mode 100644 index 00000000000..a0d6fffde6e --- /dev/null +++ b/src/systems/label/Label.hh @@ -0,0 +1,59 @@ +/* + * 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_LABEL_HH_ +#define IGNITION_GAZEBO_SYSTEMS_LABEL_HH_ + +#include + +#include "ignition/gazebo/config.hh" +#include "ignition/gazebo/System.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + /// \brief A label plugin that annotates models by setting the label + /// for the parent entity's visuals. The plugin can be attached to models, + /// visuals, or actors. + /// + /// Ex: "" means the visual has a label of 1 + /// Label value must be in [0-255] range + class Label: + public System, + public ISystemConfigure + { + /// \brief Constructor + public: explicit Label(); + + /// \brief Destructor + public: ~Label() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + }; + } +} +} +} +#endif diff --git a/src/systems/log/LogPlayback.cc b/src/systems/log/LogPlayback.cc index 60769353fe6..b7529705c69 100644 --- a/src/systems/log/LogPlayback.cc +++ b/src/systems/log/LogPlayback.cc @@ -17,7 +17,6 @@ #include "LogPlayback.hh" -#include #include #include @@ -74,14 +73,6 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// \return String of prepended path. public: std::string PrependLogPath(const std::string &_uri); - /// \brief Keeps track of which entity poses have updated - /// according to the given message. - /// \param[in] _msg Message containing pose updates. - /// \param[in] _clear Whether the most recently cached pose updates - /// should be deleted or not. Useful for when Parse is called multiple - /// times in the same Update cycle. - public: void Parse(const msgs::Pose_V &_msg, bool &_clear); - /// \brief Updates the ECM according to the given message. /// \param[in] _ecm Mutable ECM. /// \param[in] _msg Message containing state updates. @@ -122,10 +113,6 @@ class ignition::gazebo::systems::LogPlaybackPrivate /// plugin versions that did not record resources. False for older log files. public: bool doReplaceResourceURIs{true}; - /// \brief Saves which entity poses have changed according to the latest - /// LogPlaybackPrivate::Parse call. - public: std::unordered_map recentEntityPoseUpdates; - // \brief Saves which particle emitter emitting components have changed public: std::unordered_map prevParticleEmitterCmds; }; @@ -149,24 +136,6 @@ LogPlayback::~LogPlayback() LogPlaybackPrivate::started = false; } -////////////////////////////////////////////////// -void LogPlaybackPrivate::Parse(const msgs::Pose_V &_msg, bool &_clear) -{ - if (_clear) - this->recentEntityPoseUpdates.clear(); - - // save the new entity pose updates - for (auto i=0; i < _msg.pose_size(); ++i) - { - const auto &pose = _msg.pose(i); - this->recentEntityPoseUpdates.insert_or_assign(pose.id(), pose); - } - - // make sure that any future detected pose updates from the same Update - // cycle don't overwrite already-cached pose updates from the same cycle - _clear = false; -} - ////////////////////////////////////////////////// void LogPlaybackPrivate::Parse(EntityComponentManager &_ecm, const msgs::SerializedStateMap &_msg) @@ -519,39 +488,12 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) this->dataPtr->batch = this->dataPtr->log->QueryMessages( transport::log::AllTopics({startTime, endTime})); - msgs::Pose_V queuedPose; - - // If new pose updates are received, make sure that only the cached poses - // from a previous Update cycle are cleared. - // - // Since Parse can be called multiple times in a single Update, - // it's important to make sure that new poses from a given Update aren't - // overwritten by poses received in a later Parse call from the same Update. - // Since Parse may not be called at all for a given Update (it depends on the - // timestamp being investigated from the log file), we will only clear cached - // poses from a previous Update if there are new poses to be saved in the - // current Update (we know that there are new poses to be saved if Parse - // is called). - bool clearCachedPoseUpdates = true; - auto iter = this->dataPtr->batch.begin(); while (iter != this->dataPtr->batch.end()) { auto msgType = iter->Type(); - // Only set the last pose of a sequence of poses. - if (msgType != "ignition.msgs.Pose_V" && queuedPose.pose_size() > 0) - { - this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); - queuedPose.Clear(); - } - - if (msgType == "ignition.msgs.Pose_V") - { - // Queue poses to be set later - queuedPose.ParseFromString(iter->Data()); - } - else if (msgType == "ignition.msgs.SerializedState") + if (msgType == "ignition.msgs.SerializedState") { msgs::SerializedState msg; msg.ParseFromString(iter->Data()); @@ -617,29 +559,6 @@ void LogPlayback::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) ++iter; } - if (queuedPose.pose_size() > 0) - { - this->dataPtr->Parse(queuedPose, clearCachedPoseUpdates); - } - - // flag changed entity poses as periodically changed based on - // the latest LogPlaybackPrivate::Parse results - _ecm.Each( - [&](const Entity &_entity, components::Pose *_poseComp) -> bool - { - if (this->dataPtr->recentEntityPoseUpdates.find(_entity) == - this->dataPtr->recentEntityPoseUpdates.end()) - return true; - - msgs::Pose pose = this->dataPtr->recentEntityPoseUpdates.at(_entity); - *_poseComp = components::Pose(msgs::Convert(pose)); - - _ecm.SetChanged(_entity, components::Pose::typeId, - ComponentState::PeriodicChange); - - return true; - }); - // particle emitters _ecm.Each( [&](const Entity &_entity, diff --git a/src/systems/log/LogRecord.cc b/src/systems/log/LogRecord.cc index ddff8887bf3..87995ca0d08 100644 --- a/src/systems/log/LogRecord.cc +++ b/src/systems/log/LogRecord.cc @@ -54,7 +54,6 @@ #include "ignition/gazebo/components/Material.hh" #include "ignition/gazebo/components/Model.hh" #include "ignition/gazebo/components/Name.hh" -#include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/SourceFilePath.hh" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -320,13 +319,8 @@ bool LogRecordPrivate::Start(const std::string &_logPath, ignmsg << "Recording to log file [" << dbPath << "]" << std::endl; // Add default topics if no topics were specified. - std::string dynPoseTopic = "/world/" + this->worldName + - "/dynamic_pose/info"; - - igndbg << "Recording default topic[" << dynPoseTopic << "].\n"; igndbg << "Recording default topic[" << sdfTopic << "].\n"; igndbg << "Recording default topic[" << stateTopic << "].\n"; - this->recorder.AddTopic(dynPoseTopic); this->recorder.AddTopic(sdfTopic); this->recorder.AddTopic(stateTopic); diff --git a/src/systems/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 9a16fcf40c0..f9d2ac6215c 100644 --- a/src/systems/logical_camera/LogicalCamera.cc +++ b/src/systems/logical_camera/LogicalCamera.cc @@ -59,6 +59,20 @@ class ignition::gazebo::systems::LogicalCameraPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// True if the rendering component is initialized + public: bool initialized = false; + + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _logicalCamera LogicalCamera component. + /// \param[in] _parent Parent entity component. + public: void AddLogicalCamera( + EntityComponentManager &_ecm, + const Entity _entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent); + /// \brief Create logicalCamera sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateLogicalCameraEntities(EntityComponentManager &_ecm); @@ -121,55 +135,81 @@ void LogicalCamera::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveLogicalCameraEntities(_ecm); } +////////////////////////////////////////////////// +void LogicalCameraPrivate::AddLogicalCamera( + EntityComponentManager &_ecm, + const Entity _entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent) +{ + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + auto data = _logicalCamera->Data()->Clone(); + data->GetAttribute("name")->Set(sensorScopedName); + // check topic + if (!data->HasElement("topic")) + { + std::string topic = scopedName(_entity, _ecm) + "/logical_camera"; + data->GetElement("topic")->Set(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::LogicalCameraSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set sensor world pose + math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); + sensor->SetPose(sensorWorldPose); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + ////////////////////////////////////////////////// void LogicalCameraPrivate::CreateLogicalCameraEntities( EntityComponentManager &_ecm) { IGN_PROFILE("LogicalCameraPrivate::CreateLogicalCameraEntities"); - // Create logicalCameras - _ecm.EachNew( - [&](const Entity &_entity, - const components::LogicalCamera *_logicalCamera, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - auto data = _logicalCamera->Data()->Clone(); - data->GetAttribute("name")->Set(sensorScopedName); - // check topic - if (!data->HasElement("topic")) - { - std::string topic = scopedName(_entity, _ecm) + "/logical_camera"; - data->GetElement("topic")->Set(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::LogicalCameraSensor>(data); - if (nullptr == sensor) + if (!this->initialized) + { + // Create logicalCameras + _ecm.Each( + [&](const Entity &_entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set sensor world pose - math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); - sensor->SetPose(sensorWorldPose); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + }); + this->initialized = true; - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; - }); + } + else + { + // Create logicalCameras + _ecm.EachNew( + [&](const Entity &_entity, + const components::LogicalCamera *_logicalCamera, + const components::ParentEntity *_parent)->bool + { + AddLogicalCamera(_ecm, _entity, _logicalCamera, _parent); + return true; + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 227558414ba..62ce17a6080 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -56,6 +56,22 @@ class ignition::gazebo::systems::MagnetometerPrivate /// \brief Ign-sensors sensor factory for creating sensors public: sensors::SensorFactory sensorFactory; + /// True if the rendering component is initialized + public: bool initialized = false; + + /// \brief Create sensor + /// \param[in] _ecm Mutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _magnetometer Magnetometer component. + /// \param[in] _worldField MagneticField component. + /// \param[in] _parent Parent entity component. + public: void AddMagnetometer( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Magnetometer *_magnetometer, + const components::MagneticField *_worldField, + const components::ParentEntity *_parent); + /// \brief Create magnetometer sensor /// \param[in] _ecm Mutable reference to ECM. public: void CreateMagnetometerEntities(EntityComponentManager &_ecm); @@ -118,6 +134,57 @@ void Magnetometer::PostUpdate(const UpdateInfo &_info, this->dataPtr->RemoveMagnetometerEntities(_ecm); } +////////////////////////////////////////////////// +void MagnetometerPrivate::AddMagnetometer( + EntityComponentManager &_ecm, + const Entity _entity, + const components::Magnetometer *_magnetometer, + const components::MagneticField *_worldField, + const components::ParentEntity *_parent) +{ + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _magnetometer->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/magnetometer"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::MagnetometerSensor>(data); + if (nullptr == sensor) + { + ignerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // set world magnetic field. Assume uniform in world and does not + // change throughout simulation + sensor->SetWorldMagneticField(_worldField->Data()); + + // Get initial pose of sensor and set the reference z pos + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + math::Pose3d p = worldPose(_entity, _ecm); + sensor->SetWorldPose(p); + + // Set topic + _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); +} + ////////////////////////////////////////////////// void MagnetometerPrivate::CreateMagnetometerEntities( EntityComponentManager &_ecm) @@ -138,56 +205,31 @@ void MagnetometerPrivate::CreateMagnetometerEntities( return; } - // Create magnetometers - _ecm.EachNew( - [&](const Entity &_entity, - const components::Magnetometer *_magnetometer, - const components::ParentEntity *_parent)->bool - { - // create sensor - std::string sensorScopedName = - removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); - sdf::Sensor data = _magnetometer->Data(); - data.SetName(sensorScopedName); - // check topic - if (data.Topic().empty()) + if (!this->initialized) + { + // Create magnetometers + _ecm.Each( + [&](const Entity &_entity, + const components::Magnetometer *_magnetometer, + const components::ParentEntity *_parent)->bool { - std::string topic = scopedName(_entity, _ecm) + "/magnetometer"; - data.SetTopic(topic); - } - std::unique_ptr sensor = - this->sensorFactory.CreateSensor< - sensors::MagnetometerSensor>(data); - if (nullptr == sensor) + AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create magnetometers + _ecm.EachNew( + [&](const Entity &_entity, + const components::Magnetometer *_magnetometer, + const components::ParentEntity *_parent)->bool { - ignerr << "Failed to create sensor [" << sensorScopedName << "]" - << std::endl; + AddMagnetometer(_ecm, _entity, _magnetometer, worldField, _parent); return true; - } - - // set sensor parent - std::string parentName = _ecm.Component( - _parent->Data())->Data(); - sensor->SetParent(parentName); - - // set world magnetic field. Assume uniform in world and does not - // change throughout simulation - sensor->SetWorldMagneticField(worldField->Data()); - - // Get initial pose of sensor and set the reference z pos - // The WorldPose component was just created and so it's empty - // We'll compute the world pose manually here - math::Pose3d p = worldPose(_entity, _ecm); - sensor->SetWorldPose(p); - - // Set topic - _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - - this->entitySensorMap.insert( - std::make_pair(_entity, std::move(sensor))); - - return true; - }); + }); + } } ////////////////////////////////////////////////// diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 61a6312c0df..3a591d75f58 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -63,6 +64,9 @@ class ignition::gazebo::systems::OdometryPublisherPrivate /// robot base. public: std::string robotBaseFrame; + /// \brief Number of dimensions to represent odometry. + public: int dimensions; + /// \brief Update period calculated from . public: std::chrono::steady_clock::duration odomPubPeriod{0}; @@ -73,10 +77,12 @@ class ignition::gazebo::systems::OdometryPublisherPrivate public: transport::Node::Publisher odomPub; /// \brief Rolling mean accumulators for the linear velocity - public: std::pair linearMean; + public: std::tuple + linearMean; /// \brief Rolling mean accumulators for the angular velocity - public: math::RollingMean angularMean; + public: std::tuple + angularMean; /// \brief Initialized flag. public: bool initialized{false}; @@ -92,12 +98,22 @@ class ignition::gazebo::systems::OdometryPublisherPrivate OdometryPublisher::OdometryPublisher() : dataPtr(std::make_unique()) { - this->dataPtr->linearMean.first.SetWindowSize(10); - this->dataPtr->linearMean.second.SetWindowSize(10); - this->dataPtr->angularMean.SetWindowSize(10); - this->dataPtr->linearMean.first.Clear(); - this->dataPtr->linearMean.second.Clear(); - this->dataPtr->angularMean.Clear(); + std::get<0>(this->dataPtr->linearMean).SetWindowSize(10); + std::get<1>(this->dataPtr->linearMean).SetWindowSize(10); + std::get<2>(this->dataPtr->angularMean).SetWindowSize(10); + std::get<0>(this->dataPtr->linearMean).Clear(); + std::get<1>(this->dataPtr->linearMean).Clear(); + std::get<2>(this->dataPtr->angularMean).Clear(); + + if (this->dataPtr->dimensions == 3) + { + std::get<2>(this->dataPtr->linearMean).SetWindowSize(10); + std::get<0>(this->dataPtr->angularMean).SetWindowSize(10); + std::get<1>(this->dataPtr->angularMean).SetWindowSize(10); + std::get<2>(this->dataPtr->linearMean).Clear(); + std::get<0>(this->dataPtr->angularMean).Clear(); + std::get<1>(this->dataPtr->angularMean).Clear(); + } } ////////////////////////////////////////////////// @@ -110,8 +126,8 @@ void OdometryPublisher::Configure(const Entity &_entity, if (!this->dataPtr->model.Valid(_ecm)) { - ignerr << "DiffDrive plugin should be attached to a model entity. " - << "Failed to initialize." << std::endl; + ignerr << "OdometryPublisher system plugin should be attached to a model" + << " entity. Failed to initialize." << std::endl; return; } @@ -138,6 +154,24 @@ void OdometryPublisher::Configure(const Entity &_entity, this->dataPtr->robotBaseFrame = _sdf->Get("robot_base_frame"); } + this->dataPtr->dimensions = 2; + if (!_sdf->HasElement("dimensions")) + { + igndbg << "OdometryPublisher system plugin missing , " + << "defaults to \"" << this->dataPtr->dimensions << "\"" << std::endl; + } + else + { + this->dataPtr->dimensions = _sdf->Get("dimensions"); + if (this->dataPtr->dimensions != 2 && this->dataPtr->dimensions != 3) + { + ignerr << "OdometryPublisher system plugin must be 2D or 3D " + << "not " << this->dataPtr->dimensions + << "D. Failed to initialize." << std::endl; + return; + } + } + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; if (odomFreq > 0) { @@ -203,7 +237,7 @@ void OdometryPublisherPrivate::UpdateOdometry( const ignition::gazebo::UpdateInfo &_info, const ignition::gazebo::EntityComponentManager &_ecm) { - IGN_PROFILE("DiffDrive::UpdateOdometry"); + IGN_PROFILE("OdometryPublisher::UpdateOdometry"); // Record start time. if (!this->initialized) { @@ -227,6 +261,10 @@ void OdometryPublisherPrivate::UpdateOdometry( msg.mutable_pose()->mutable_position()->set_x(pose.Pos().X()); msg.mutable_pose()->mutable_position()->set_y(pose.Pos().Y()); msgs::Set(msg.mutable_pose()->mutable_orientation(), pose.Rot()); + if (this->dimensions == 3) + { + msg.mutable_pose()->mutable_position()->set_z(pose.Pos().Z()); + } // Get linear and angular displacements from last updated pose. double linearDisplacementX = pose.Pos().X() - this->lastUpdatePose.Pos().X(); @@ -236,19 +274,64 @@ void OdometryPublisherPrivate::UpdateOdometry( const double lastYaw = this->lastUpdatePose.Rot().Yaw(); while (currentYaw < lastYaw - IGN_PI) currentYaw += 2 * IGN_PI; while (currentYaw > lastYaw + IGN_PI) currentYaw -= 2 * IGN_PI; - const float angularDiff = currentYaw - lastYaw; - - // Get velocities in robotBaseFrame and add to message. - double linearVelocityX = (cosf(currentYaw) * linearDisplacementX - + sinf(currentYaw) * linearDisplacementY) / dt.count(); - double linearVelocityY = (cosf(currentYaw) * linearDisplacementY - - sinf(currentYaw) * linearDisplacementX) / dt.count(); - this->linearMean.first.Push(linearVelocityX); - this->linearMean.second.Push(linearVelocityY); - this->angularMean.Push(angularDiff / dt.count()); - msg.mutable_twist()->mutable_linear()->set_x(this->linearMean.first.Mean()); - msg.mutable_twist()->mutable_linear()->set_y(this->linearMean.second.Mean()); - msg.mutable_twist()->mutable_angular()->set_z(this->angularMean.Mean()); + const float yawDiff = currentYaw - lastYaw; + + // Get velocities assuming 2D + if (this->dimensions == 2) + { + double linearVelocityX = (cosf(currentYaw) * linearDisplacementX + + sinf(currentYaw) * linearDisplacementY) / dt.count(); + double linearVelocityY = (cosf(currentYaw) * linearDisplacementY + - sinf(currentYaw) * linearDisplacementX) / dt.count(); + std::get<0>(this->linearMean).Push(linearVelocityX); + std::get<1>(this->linearMean).Push(linearVelocityY); + msg.mutable_twist()->mutable_linear()->set_x( + std::get<0>(this->linearMean).Mean()); + msg.mutable_twist()->mutable_linear()->set_y( + std::get<1>(this->linearMean).Mean()); + } + // Get velocities and roll/pitch rates assuming 3D + else if (this->dimensions == 3) + { + double currentRoll = pose.Rot().Roll(); + const double lastRoll = this->lastUpdatePose.Rot().Roll(); + while (currentRoll < lastRoll - IGN_PI) currentRoll += 2 * IGN_PI; + while (currentRoll > lastRoll + IGN_PI) currentRoll -= 2 * IGN_PI; + const float rollDiff = currentRoll - lastRoll; + + double currentPitch = pose.Rot().Pitch(); + const double lastPitch = this->lastUpdatePose.Rot().Pitch(); + while (currentPitch < lastPitch - IGN_PI) currentPitch += 2 * IGN_PI; + while (currentPitch > lastPitch + IGN_PI) currentPitch -= 2 * IGN_PI; + const float pitchDiff = currentPitch - lastPitch; + + double linearDisplacementZ = + pose.Pos().Z() - this->lastUpdatePose.Pos().Z(); + math::Vector3 linearDisplacement(linearDisplacementX, linearDisplacementY, + linearDisplacementZ); + math::Vector3 linearVelocity = + pose.Rot().RotateVectorReverse(linearDisplacement) / dt.count(); + std::get<0>(this->linearMean).Push(linearVelocity.X()); + std::get<1>(this->linearMean).Push(linearVelocity.Y()); + std::get<2>(this->linearMean).Push(linearVelocity.Z()); + std::get<0>(this->angularMean).Push(rollDiff / dt.count()); + std::get<1>(this->angularMean).Push(pitchDiff / dt.count()); + msg.mutable_twist()->mutable_linear()->set_x( + std::get<0>(this->linearMean).Mean()); + msg.mutable_twist()->mutable_linear()->set_y( + std::get<1>(this->linearMean).Mean()); + msg.mutable_twist()->mutable_linear()->set_z( + std::get<2>(this->linearMean).Mean()); + msg.mutable_twist()->mutable_angular()->set_x( + std::get<0>(this->angularMean).Mean()); + msg.mutable_twist()->mutable_angular()->set_y( + std::get<1>(this->angularMean).Mean()); + } + + // Set yaw rate + std::get<2>(this->angularMean).Push(yawDiff / dt.count()); + msg.mutable_twist()->mutable_angular()->set_z( + std::get<2>(this->angularMean).Mean()); // Set the time stamp in the header. msg.mutable_header()->mutable_stamp()->CopyFrom( diff --git a/src/systems/odometry_publisher/OdometryPublisher.hh b/src/systems/odometry_publisher/OdometryPublisher.hh index f60e309d063..832d3ac3014 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.hh +++ b/src/systems/odometry_publisher/OdometryPublisher.hh @@ -33,7 +33,7 @@ namespace systems class OdometryPublisherPrivate; /// \brief Odometry Publisher which can be attached to any entity in - /// order to periodically publish 2D odometry data in the form of + /// order to periodically publish 2D or 3D odometry data in the form of /// ignition::msgs::Odometry messages. /// /// # System Parameters @@ -52,6 +52,10 @@ namespace systems /// ``: Custom topic on which this system will publish odometry /// messages. This element is optional, and the default value is /// `/model/{name_of_model}/odometry`. + /// + /// ``: Number of dimensions to represent odometry. Only 2 and 3 + /// dimensional spaces are supported. This element is optional, and the + /// default value is 2. class OdometryPublisher : public System, public ISystemConfigure, diff --git a/src/systems/particle_emitter/ParticleEmitter.hh b/src/systems/particle_emitter/ParticleEmitter.hh index 935fbd0eb2b..6a3da57d233 100644 --- a/src/systems/particle_emitter/ParticleEmitter.hh +++ b/src/systems/particle_emitter/ParticleEmitter.hh @@ -119,7 +119,7 @@ namespace systems public ISystemPreUpdate { /// \brief Constructor - public: ParticleEmitter(); + public: IGN_DEPRECATED(6) ParticleEmitter(); // Documentation inherited public: void Configure(const Entity &_entity, diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 1811d1ffff8..faa114c8ffb 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -47,6 +47,7 @@ #include #include +#include #include #include #include @@ -101,11 +102,14 @@ #include "ignition/gazebo/components/DetachableJoint.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointEffortLimitsCmd.hh" #include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh" #include "ignition/gazebo/components/JointVelocityReset.hh" #include "ignition/gazebo/components/LinearAcceleration.hh" #include "ignition/gazebo/components/LinearVelocity.hh" @@ -116,6 +120,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" #include "ignition/gazebo/components/JointForceCmd.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" @@ -129,6 +134,9 @@ #include "ignition/gazebo/components/HaltMotion.hh" #include "CanonicalLinkModelTracker.hh" +// Events +#include "ignition/gazebo/physics/Events.hh" + #include "EntityFeatureMap.hh" using namespace ignition; @@ -284,6 +292,14 @@ class ignition::gazebo::systems::PhysicsPrivate public: ignition::math::Pose3d RelativePose(const Entity &_from, const Entity &_to, const EntityComponentManager &_ecm) const; + /// \brief Enable contact surface customization for the given world. + /// \param[in] _world The world to enable it for. + public: void EnableContactSurfaceCustomization(const Entity &_world); + + /// \brief Disable contact surface customization for the given world. + /// \param[in] _world The world to disable it for. + public: void DisableContactSurfaceCustomization(const Entity &_world); + /// \brief Cache the top-level model for each entity. /// The key is an entity and the value is its top level model. public: std::unordered_map topLevelModelMap; @@ -314,6 +330,9 @@ class ignition::gazebo::systems::PhysicsPrivate /// deleted the following iteration. public: std::unordered_set worldPoseCmdsToRemove; + /// \brief IDs of the ContactSurfaceHandler callbacks registered for worlds + public: std::unordered_map worldContactCallbackIDs; + /// \brief used to store whether physics objects have been created. public: bool initialized = false; @@ -379,6 +398,19 @@ class ignition::gazebo::systems::PhysicsPrivate } return true; }}; + /// \brief msgs::Contacts equality comparison function. + public: std::function + wrenchEql{ + [](const msgs::Wrench &_a, const msgs::Wrench &_b) + { + return math::equal(_a.torque().x(), _b.torque().x(), 1e-6) && + math::equal(_a.torque().y(), _b.torque().y(), 1e-6) && + math::equal(_a.torque().z(), _b.torque().z(), 1e-6) && + + math::equal(_a.force().x(), _b.force().x(), 1e-6) && + math::equal(_a.force().y(), _b.force().y(), 1e-6) && + math::equal(_a.force().z(), _b.force().z(), 1e-6); + }}; /// \brief Environment variable which holds paths to look for engine plugins public: std::string pluginPathEnv = "IGN_GAZEBO_PHYSICS_ENGINE_PATH"; @@ -418,6 +450,12 @@ class ignition::gazebo::systems::PhysicsPrivate physics::DetachJointFeature, physics::SetJointTransformFromParentFeature>{}; + ////////////////////////////////////////////////// + // Joint transmitted wrench + /// \brief Feature list for getting joint transmitted wrenches. + public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList< + physics::GetJointTransmittedWrench>{}; + ////////////////////////////////////////////////// // Collisions @@ -431,6 +469,12 @@ class ignition::gazebo::systems::PhysicsPrivate CollisionFeatureList, ignition::physics::GetContactsFromLastStepFeature>{}; + /// \brief Feature list to change contacts before they are applied to physics. + public: struct SetContactPropertiesCallbackFeatureList : + ignition::physics::FeatureList< + ContactFeatureList, + ignition::physics::SetContactPropertiesCallbackFeature>{}; + /// \brief Collision type with collision features. public: using ShapePtrType = ignition::physics::ShapePtr< ignition::physics::FeaturePolicy3d, CollisionFeatureList>; @@ -468,6 +512,28 @@ class ignition::gazebo::systems::PhysicsPrivate public: struct JointVelocityCommandFeatureList : physics::FeatureList< physics::SetJointVelocityCommandFeature>{}; + + ////////////////////////////////////////////////// + // Joint position limits command + /// \brief Feature list for setting joint position limits. + public: struct JointPositionLimitsCommandFeatureList : physics::FeatureList< + physics::SetJointPositionLimitsFeature>{}; + + + ////////////////////////////////////////////////// + // Joint velocity limits command + /// \brief Feature list for setting joint velocity limits. + public: struct JointVelocityLimitsCommandFeatureList : physics::FeatureList< + physics::SetJointVelocityLimitsFeature>{}; + + + ////////////////////////////////////////////////// + // Joint effort limits command + /// \brief Feature list for setting joint effort limits. + public: struct JointEffortLimitsCommandFeatureList : physics::FeatureList< + physics::SetJointEffortLimitsFeature>{}; + + ////////////////////////////////////////////////// // World velocity command public: struct WorldVelocityCommandFeatureList : @@ -522,6 +588,7 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, CollisionFeatureList, ContactFeatureList, + SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList>; @@ -561,7 +628,11 @@ class ignition::gazebo::systems::PhysicsPrivate physics::Joint, JointFeatureList, DetachableJointFeatureList, - JointVelocityCommandFeatureList + JointVelocityCommandFeatureList, + JointGetTransmittedWrenchFeatureList, + JointPositionLimitsCommandFeatureList, + JointVelocityLimitsCommandFeatureList, + JointEffortLimitsCommandFeatureList >; /// \brief A map between joint entity ids in the ECM to Joint Entities in @@ -591,6 +662,15 @@ class ignition::gazebo::systems::PhysicsPrivate /// \brief A map between collision entity ids in the ECM to FreeGroup Entities /// in ign-physics. public: EntityFreeGroupMap entityFreeGroupMap; + + /// \brief Event manager from simulation runner. + public: EventManager *eventManager = nullptr; + + /// \brief Keep track of what entities use customized contact surfaces. + /// Map keys are expected to be world entities so that we keep a set of + /// entities with customizations per world. + public: std::unordered_map> + customContactSurfaceEntities; }; ////////////////////////////////////////////////// @@ -602,7 +682,7 @@ Physics::Physics() : System(), dataPtr(std::make_unique()) void Physics::Configure(const Entity &_entity, const std::shared_ptr &_sdf, EntityComponentManager &_ecm, - EventManager &/*_eventMgr*/) + EventManager &_eventMgr) { std::string pluginLib; @@ -715,7 +795,10 @@ void Physics::Configure(const Entity &_entity, ignerr << "Failed to load a valid physics engine from [" << pathToLib << "]." << std::endl; + return; } + + this->dataPtr->eventManager = &_eventMgr; } ////////////////////////////////////////////////// @@ -1402,6 +1485,41 @@ void PhysicsPrivate::CreateJointEntities(const EntityComponentManager &_ecm) } return true; }); + + // The components are removed after each update, so we want to process all + // components in every update. + _ecm.Each( + [&](const Entity & _entity, + const components::EnableContactSurfaceCustomization *_enable, + const components::Collision */*_collision*/, + const components::Name *_name) -> bool + { + const auto world = worldEntity(_entity, _ecm); + if (_enable->Data()) + { + if (this->customContactSurfaceEntities[world].empty()) + { + this->EnableContactSurfaceCustomization(world); + } + this->customContactSurfaceEntities[world].insert(_entity); + ignmsg << "Enabling contact surface customization for collision [" + << _name->Data() << "]" << std::endl; + } + else + { + if (this->customContactSurfaceEntities[world].erase(_entity) > 0) + { + ignmsg << "Disabling contact surface customization for collision [" + << _name->Data() << "]" << std::endl; + if (this->customContactSurfaceEntities[world].empty()) + { + this->DisableContactSurfaceCustomization(world); + } + } + } + return true; + }); } ////////////////////////////////////////////////// @@ -1431,6 +1549,7 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) [&](const Entity &_entity, const components::Model * /* _model */) -> bool { + const auto world = worldEntity(_ecm); // Remove model if found if (auto modelPtrPhys = this->entityModelMap.Get(_entity)) { @@ -1443,6 +1562,16 @@ void PhysicsPrivate::RemovePhysicsEntities(const EntityComponentManager &_ecm) { this->entityCollisionMap.Remove(childCollision); this->topLevelModelMap.erase(childCollision); + if (this->customContactSurfaceEntities[world].erase( + childCollision)) + { + // if this was the last collision with contact customization, + // disable the whole feature in the physics engine + if (this->customContactSurfaceEntities[world].empty()) + { + this->DisableContactSurfaceCustomization(world); + } + } } this->entityLinkMap.Remove(childLink); this->topLevelModelMap.erase(childLink); @@ -1532,6 +1661,18 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) this->entityJointMap.EntityCast( _entity); + auto jointPosLimitsFeature = + this->entityJointMap.EntityCast + (_entity); + + auto jointVelLimitsFeature = + this->entityJointMap.EntityCast + (_entity); + + auto jointEffLimitsFeature = + this->entityJointMap.EntityCast( + _entity); + auto haltMotionComp = _ecm.Component( _ecm.ParentEntity(_entity)); bool haltMotion = false; @@ -1557,6 +1698,99 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; } + auto posLimits = _ecm.Component( + _entity); + if (posLimits && !posLimits->Data().empty()) + { + const auto& limits = posLimits->Data(); + + if (limits.size() != jointPhys->GetDegreesOfFreedom()) + { + ignwarn << "There is a mismatch in the degrees of freedom " + << "between Joint [" << _name->Data() << "(Entity=" + << _entity << ")] and its JointPositionLimitsCmd " + << "component. The joint has " + << jointPhys->GetDegreesOfFreedom() + << " while the component has " + << limits.size() << ".\n"; + } + + if (jointPosLimitsFeature) + { + std::size_t nDofs = std::min( + limits.size(), + jointPhys->GetDegreesOfFreedom()); + + for (std::size_t i = 0; i < nDofs; ++i) + { + jointPosLimitsFeature->SetMinPosition(i, limits[i].X()); + jointPosLimitsFeature->SetMaxPosition(i, limits[i].Y()); + } + } + } + + auto velLimits = _ecm.Component( + _entity); + if (velLimits && !velLimits->Data().empty()) + { + const auto& limits = velLimits->Data(); + + if (limits.size() != jointPhys->GetDegreesOfFreedom()) + { + ignwarn << "There is a mismatch in the degrees of freedom " + << "between Joint [" << _name->Data() << "(Entity=" + << _entity << ")] and its JointVelocityLimitsCmd " + << "component. The joint has " + << jointPhys->GetDegreesOfFreedom() + << " while the component has " + << limits.size() << ".\n"; + } + + if (jointVelLimitsFeature) + { + std::size_t nDofs = std::min( + limits.size(), + jointPhys->GetDegreesOfFreedom()); + + for (std::size_t i = 0; i < nDofs; ++i) + { + jointVelLimitsFeature->SetMinVelocity(i, limits[i].X()); + jointVelLimitsFeature->SetMaxVelocity(i, limits[i].Y()); + } + } + } + + auto effLimits = _ecm.Component( + _entity); + if (effLimits && !effLimits->Data().empty()) + { + const auto& limits = effLimits->Data(); + + if (limits.size() != jointPhys->GetDegreesOfFreedom()) + { + ignwarn << "There is a mismatch in the degrees of freedom " + << "between Joint [" << _name->Data() << "(Entity=" + << _entity << ")] and its JointEffortLimitsCmd " + << "component. The joint has " + << jointPhys->GetDegreesOfFreedom() + << " while the component has " + << limits.size() << ".\n"; + } + + if (jointEffLimitsFeature) + { + std::size_t nDofs = std::min( + limits.size(), + jointPhys->GetDegreesOfFreedom()); + + for (std::size_t i = 0; i < nDofs; ++i) + { + jointEffLimitsFeature->SetMinEffort(i, limits[i].X()); + jointEffLimitsFeature->SetMaxEffort(i, limits[i].Y()); + } + } + } + auto posReset = _ecm.Component( _entity); auto velReset = _ecm.Component( @@ -2063,7 +2297,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) return true; }); -} +} // NOLINT readability/fn_size +// TODO (azeey) Reduce size of function and remove the NOLINT above ////////////////////////////////////////////////// ignition::physics::ForwardStep::Output PhysicsPrivate::Step( @@ -2287,7 +2522,9 @@ void PhysicsPrivate::UpdateModelPose(const Entity _model, _ecm.Component(nestedModel); if (!nestedModelCanonicalLinkComp) { - ignerr << "Model [" << nestedModel << "] has no canonical link\n"; + auto staticComp = _ecm.Component(nestedModel); + if (!staticComp || !staticComp->Data()) + ignerr << "Model [" << nestedModel << "] has no canonical link\n"; continue; } @@ -2695,6 +2932,20 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, _ecm.RemoveComponent(entity); } + std::vector entitiesCustomContactSurface; + _ecm.Each( + [&](const Entity &_entity, + components::EnableContactSurfaceCustomization *) -> bool + { + entitiesCustomContactSurface.push_back(_entity); + return true; + }); + + for (const auto entity : entitiesCustomContactSurface) + { + _ecm.RemoveComponent(entity); + } + // Clear pending commands _ecm.Each( [&](const Entity &, components::JointForceCmd *_force) -> bool @@ -2710,6 +2961,27 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); + _ecm.Each( + [&](const Entity &, components::JointPositionLimitsCmd *_limits) -> bool + { + _limits->Data().clear(); + return true; + }); + + _ecm.Each( + [&](const Entity &, components::JointVelocityLimitsCmd *_limits) -> bool + { + _limits->Data().clear(); + return true; + }); + + _ecm.Each( + [&](const Entity &, components::JointEffortLimitsCmd *_limits) -> bool + { + _limits->Data().clear(); + return true; + }); + _ecm.Each( [&](const Entity &, components::JointVelocityCmd *_vel) -> bool { @@ -2748,8 +3020,7 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, if (auto jointPhys = this->entityJointMap.Get(_entity)) { _jointPos->Data().resize(jointPhys->GetDegreesOfFreedom()); - for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); - ++i) + for (std::size_t i = 0; i < jointPhys->GetDegreesOfFreedom(); ++i) { _jointPos->Data()[i] = jointPhys->GetPosition(i); } @@ -2777,6 +3048,46 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, }); IGN_PROFILE_END(); + // Update joint transmitteds + _ecm.Each( + [&](const Entity &_entity, components::Joint *, + components::JointTransmittedWrench *_wrench) -> bool + { + auto jointPhys = + this->entityJointMap + .EntityCast(_entity); + if (jointPhys) + { + const auto &jointWrench = jointPhys->GetTransmittedWrench(); + + msgs::Wrench wrenchData; + msgs::Set(wrenchData.mutable_torque(), + math::eigen3::convert(jointWrench.torque)); + msgs::Set(wrenchData.mutable_force(), + math::eigen3::convert(jointWrench.force)); + const auto state = + _wrench->SetData(wrenchData, this->wrenchEql) + ? ComponentState::PeriodicChange + : ComponentState::NoChange; + _ecm.SetChanged(_entity, components::JointTransmittedWrench::typeId, + state); + } + else + { + static bool informed{false}; + if (!informed) + { + igndbg + << "Attempting to get joint transmitted wrenches, but the " + "physics engine doesn't support this feature. Values in the " + "JointTransmittedWrench component will not be meaningful." + << std::endl; + informed = true; + } + } + return true; + }); + // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); } @@ -2901,6 +3212,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( const LinkPtrType &_link, const math::Pose3d &_pose) const { @@ -2910,6 +3222,97 @@ physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( return this->engine->Resolve(relFrameData, physics::FrameID::World()); } +////////////////////////////////////////////////// +void PhysicsPrivate::EnableContactSurfaceCustomization(const Entity &_world) +{ + // allow customization of contact joint surface parameters + auto setContactPropertiesCallbackFeature = + this->entityWorldMap.EntityCast< + SetContactPropertiesCallbackFeatureList>(_world); + if (!setContactPropertiesCallbackFeature) + return; + + using Policy = physics::FeaturePolicy3d; + using Feature = physics::SetContactPropertiesCallbackFeature; + using FeatureList = SetContactPropertiesCallbackFeatureList; + using GCFeature = physics::GetContactsFromLastStepFeature; + using GCFeatureWorld = GCFeature::World; + using ContactPoint = GCFeatureWorld::ContactPoint; + using ExtraContactData = GCFeature::ExtraContactDataT; + + const auto callbackID = "ignition::gazebo::systems::Physics"; + setContactPropertiesCallbackFeature->AddContactPropertiesCallback( + callbackID, + [this, _world](const GCFeatureWorld::Contact &_contact, + const size_t _numContactsOnCollision, + Feature::ContactSurfaceParams &_params) + { + const auto &contact = _contact.Get(); + auto coll1Entity = this->entityCollisionMap.Get( + ShapePtrType(contact.collision1)); + auto coll2Entity = this->entityCollisionMap.Get( + ShapePtrType(contact.collision2)); + + // check if at least one of the entities wants contact surface + // customization + if (this->customContactSurfaceEntities[_world].find(coll1Entity) == + this->customContactSurfaceEntities[_world].end() && + this->customContactSurfaceEntities[_world].find(coll2Entity) == + this->customContactSurfaceEntities[_world].end()) + { + return; + } + + std::optional force; + std::optional normal; + std::optional depth; + const auto* extraData = _contact.Query(); + if (extraData != nullptr) + { + force = math::eigen3::convert(extraData->force); + normal = math::eigen3::convert(extraData->normal); + depth = extraData->depth; + } + + // broadcast the event that we want to collect the customized + // contact surface properties; each connected client should + // filter in the callback to treat just the entities it knows + this->eventManager-> + Emit( + coll1Entity, coll2Entity, math::eigen3::convert(contact.point), + force, normal, depth, _numContactsOnCollision, _params); + } + ); + + this->worldContactCallbackIDs[_world] = callbackID; + + ignmsg << "Enabled contact surface customization for world entity [" << _world + << "]" << std::endl; +} + + +////////////////////////////////////////////////// +void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) +{ + if (this->worldContactCallbackIDs.find(_world) == + this->worldContactCallbackIDs.end()) + { + return; + } + + auto setContactPropertiesCallbackFeature = + this->entityWorldMap.EntityCast< + SetContactPropertiesCallbackFeatureList>(_world); + if (!setContactPropertiesCallbackFeature) + return; + + setContactPropertiesCallbackFeature-> + RemoveContactPropertiesCallback(this->worldContactCallbackIDs[_world]); + + ignmsg << "Disabled contact surface customization for world entity [" + << _world << "]" << std::endl; +} + IGNITION_ADD_PLUGIN(Physics, ignition::gazebo::System, Physics::ISystemConfigure, diff --git a/src/systems/sensors/CMakeLists.txt b/src/systems/sensors/CMakeLists.txt index da081b2dff4..8b56378885a 100644 --- a/src/systems/sensors/CMakeLists.txt +++ b/src/systems/sensors/CMakeLists.txt @@ -2,12 +2,16 @@ gz_add_system(sensors SOURCES Sensors.cc PUBLIC_LINK_LIBS + ${rendering_target} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} - ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} + ignition-rendering${IGN_RENDERING_VER}::ignition-rendering${IGN_RENDERING_VER} ignition-sensors${IGN_SENSORS_VER}::camera - ignition-sensors${IGN_SENSORS_VER}::gpu_lidar ignition-sensors${IGN_SENSORS_VER}::depth_camera + ignition-sensors${IGN_SENSORS_VER}::gpu_lidar + ignition-sensors${IGN_SENSORS_VER}::ignition-sensors${IGN_SENSORS_VER} + ignition-sensors${IGN_SENSORS_VER}::lidar + ignition-sensors${IGN_SENSORS_VER}::rgbd_camera + ignition-sensors${IGN_SENSORS_VER}::segmentation_camera ignition-sensors${IGN_SENSORS_VER}::thermal_camera - ${PROJECT_LIBRARY_TARGET_NAME}-rendering ) diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a20b5c32808..366d81c46e7 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -32,16 +32,22 @@ #include #include +#include +#include #include +#include #include +#include #include #include "ignition/gazebo/components/Atmosphere.hh" #include "ignition/gazebo/components/Camera.hh" #include "ignition/gazebo/components/DepthCamera.hh" #include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/RenderEngineServerHeadless.hh" #include "ignition/gazebo/components/RenderEngineServerPlugin.hh" #include "ignition/gazebo/components/RgbdCamera.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" #include "ignition/gazebo/components/ThermalCamera.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Events.hh" @@ -174,6 +180,12 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Stop the rendering thread public: void Stop(); + + /// \brief Use to optionally set the background color. + public: std::optional backgroundColor; + + /// \brief Use to optionally set the ambient light. + public: std::optional ambientLight; }; ////////////////////////////////////////////////// @@ -194,8 +206,13 @@ void SensorsPrivate::WaitForInit() { // Only initialize if there are rendering sensors igndbg << "Initializing render context" << std::endl; + if (this->backgroundColor) + this->renderUtil.SetBackgroundColor(*this->backgroundColor); + if (this->ambientLight) + this->renderUtil.SetAmbientLight(*this->ambientLight); this->renderUtil.Init(); this->scene = this->renderUtil.Scene(); + this->scene->SetCameraPassCountPerGpuFlush(6u); this->initialized = true; } @@ -262,6 +279,15 @@ void SensorsPrivate::RunOnce() // publish data IGN_PROFILE("RunOnce"); this->sensorManager.RunOnce(this->updateTime); + } + + { + IGN_PROFILE("PostRender"); + // Update the scene graph manually to improve performance + // We only need to do this once per frame It is important to call + // sensors::RenderingSensor::SetManualSceneUpdate and set it to true + // so we don't waste cycles doing one scene graph update per sensor + this->scene->PostRender(); this->eventManager->Emit(); } @@ -369,10 +395,19 @@ void Sensors::Configure(const Entity &/*_id*/, EventManager &_eventMgr) { igndbg << "Configuring Sensors system" << std::endl; + // Setup rendering std::string engineName = _sdf->Get("render_engine", "ogre2").first; + // Get the background color, if specified. + if (_sdf->HasElement("background_color")) + this->dataPtr->backgroundColor = _sdf->Get("background_color"); + + // Get the ambient light, if specified. + if (_sdf->HasElement("ambient_light")) + this->dataPtr->ambientLight = _sdf->Get("ambient_light"); + this->dataPtr->renderUtil.SetEngineName(engineName); this->dataPtr->renderUtil.SetEnableSensors(true, std::bind(&Sensors::CreateSensor, this, @@ -401,6 +436,15 @@ void Sensors::Configure(const Entity &/*_id*/, { this->dataPtr->renderUtil.SetEngineName(renderEngineServerComp->Data()); } + + // Set headless mode if specified from command line + auto renderEngineServerHeadlessComp = + _ecm.Component(worldEntity); + if (renderEngineServerHeadlessComp) + { + this->dataPtr->renderUtil.SetHeadlessRendering( + renderEngineServerHeadlessComp->Data()); + } } this->dataPtr->eventManager = &_eventMgr; @@ -417,6 +461,7 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::Update"); + std::unique_lock lock(this->dataPtr->renderMutex); if (this->dataPtr->running && this->dataPtr->initialized) { this->dataPtr->renderUtil.UpdateECM(_info, _ecm); @@ -437,17 +482,21 @@ void Sensors::PostUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } - if (!this->dataPtr->initialized && - (_ecm.HasComponentType(components::Camera::typeId) || - _ecm.HasComponentType(components::DepthCamera::typeId) || - _ecm.HasComponentType(components::GpuLidar::typeId) || - _ecm.HasComponentType(components::RgbdCamera::typeId) || - _ecm.HasComponentType(components::ThermalCamera::typeId))) + { - igndbg << "Initialization needed" << std::endl; std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->doInit = true; - this->dataPtr->renderCv.notify_one(); + if (!this->dataPtr->initialized && + (_ecm.HasComponentType(components::Camera::typeId) || + _ecm.HasComponentType(components::DepthCamera::typeId) || + _ecm.HasComponentType(components::GpuLidar::typeId) || + _ecm.HasComponentType(components::RgbdCamera::typeId) || + _ecm.HasComponentType(components::ThermalCamera::typeId) || + _ecm.HasComponentType(components::SegmentationCamera::typeId))) + { + igndbg << "Initialization needed" << std::endl; + this->dataPtr->doInit = true; + this->dataPtr->renderCv.notify_one(); + } } if (this->dataPtr->running && this->dataPtr->initialized) @@ -516,24 +565,52 @@ std::string Sensors::CreateSensor(const Entity &_entity, } // Create within ign-sensors - auto sensorId = this->dataPtr->sensorManager.CreateSensor(_sdf); - auto sensor = this->dataPtr->sensorManager.Sensor(sensorId); - - // Add to sensorID -> entity map - this->dataPtr->entityToIdMap.insert({_entity, sensorId}); + sensors::Sensor *sensor{nullptr}; + if (_sdf.Type() == sdf::SensorType::CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::CameraSensor>(_sdf); + } + else if (_sdf.Type() == sdf::SensorType::DEPTH_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::DepthCameraSensor>(_sdf); + } + else if (_sdf.Type() == sdf::SensorType::GPU_LIDAR) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::GpuLidarSensor>(_sdf); + } + else if (_sdf.Type() == sdf::SensorType::RGBD_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::RgbdCameraSensor>(_sdf); + } + else if (_sdf.Type() == sdf::SensorType::THERMAL_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::ThermalCameraSensor>(_sdf); + } + else if (_sdf.Type() == sdf::SensorType::SEGMENTATION_CAMERA) + { + sensor = this->dataPtr->sensorManager.CreateSensor< + sensors::SegmentationCameraSensor>(_sdf); + } - if (nullptr == sensor || sensors::NO_SENSOR == sensor->Id()) + if (nullptr == sensor) { ignerr << "Failed to create sensor [" << _sdf.Name() - << "]" << std::endl; + << "]." << std::endl; return std::string(); } + // Store sensor ID + auto sensorId = sensor->Id(); + this->dataPtr->entityToIdMap.insert({_entity, sensorId}); this->dataPtr->sensorIds.insert(sensorId); // Set the scene so it can create the rendering sensor - auto renderingSensor = - dynamic_cast(sensor); + auto renderingSensor = dynamic_cast(sensor); renderingSensor->SetScene(this->dataPtr->scene); renderingSensor->SetParent(_parentName); renderingSensor->SetManualSceneUpdate(true); diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index 3f988fa146a..e871b9b1806 100644 --- a/src/systems/sensors/Sensors.hh +++ b/src/systems/sensors/Sensors.hh @@ -36,7 +36,19 @@ namespace systems class SensorsPrivate; /// \class Sensors Sensors.hh ignition/gazebo/systems/Sensors.hh - /// \brief TODO(louise) Have one system for all sensors, or one per + /// \brief A system that manages sensors. + /// + /// ## System Parameters + /// + /// - `` Name of the render engine, such as 'ogre' or 'ogre2'. + /// - `` Color used for the scene's background. This + /// will override the background color specified in a world's SDF + /// element. This background color is used by sensors, not the GUI. + /// - `` Color used for the scene's ambient light. This + /// will override the ambient value specified in a world's SDF + /// element. This ambient light is used by sensors, not the GUI. + /// + /// \TODO(louise) Have one system for all sensors, or one per /// sensor / sensor type? class Sensors: public System, diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index eefd5b0e90e..6211434bc9d 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -18,18 +18,19 @@ #include #include +#include + #include #include #include -#include - #include "ignition/gazebo/components/AngularVelocity.hh" #include "ignition/gazebo/components/ChildLinkName.hh" -#include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/components/LinearVelocity.hh" #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Link.hh" @@ -50,26 +51,44 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief Thrust output by propeller in N public: double thrust = 0.0; + /// \brief Desired propeller angular velocity in rad / s + public: double propellerAngVel = 0.0; + /// \brief The link entity which will spin public: ignition::gazebo::Entity linkEntity; - /// \brief Axis along which the propeller spins + /// \brief Axis along which the propeller spins. Expressed in the joint + /// frame. Addume this doesn't change during simulation. public: ignition::math::Vector3d jointAxis; + /// \brief Joint pose in the child link frame. Assume this doesn't change + /// during the simulation. + public: math::Pose3d jointPose; + + /// \brief Propeller koint entity + public: ignition::gazebo::Entity jointEntity; + /// \brief ignition node for handling transport public: ignition::transport::Node node; - /// \brief The PID which controls the rpm - public: ignition::math::PID rpmController; + /// \brief The PID which controls the propeller. This isn't used if + /// velocityControl is true. + public: ignition::math::PID propellerController; - /// \brief maximum input force [N], default: 1000N + /// \brief Velocity Control mode - this disables the propellerController + /// and writes the angular velocity directly to the joint. default: false + public: bool velocityControl = false; + + /// \brief Maximum input force [N] for the propellerController, default: 1000N + /// TODO(chapulina) Make it configurable from SDF. public: double cmdMax = 1000; - /// \brief minimum input force [N], default: 1000N + /// \brief Minimum input force [N] for the propellerController, default: 1000N + /// TODO(chapulina) Make it configurable from SDF. public: double cmdMin = -1000; - /// \brief Thrust coefficient relating the - /// propeller rpm to the thrust + /// \brief Thrust coefficient relating the propeller angular velocity to the + /// thrust public: double thrustCoefficient = 1; /// \brief Density of fluid in kgm^-3, default: 1000kgm^-3 @@ -81,19 +100,17 @@ class ignition::gazebo::systems::ThrusterPrivateData /// \brief callback for handling thrust update public: void OnCmdThrust(const ignition::msgs::Double &_msg); - /// \brief function which computes rpm from thrust - public: double ThrustToAngularVec(double thrust); + /// \brief function which computes angular velocity from thrust + /// \param[in] _thrust Thrust in N + /// \return Angular velocity in rad/s + public: double ThrustToAngularVec(double _thrust); }; ///////////////////////////////////////////////// -Thruster::Thruster() -{ - this->dataPtr = std::make_unique(); -} - -///////////////////////////////////////////////// -Thruster::~Thruster() +Thruster::Thruster(): + dataPtr(std::make_unique()) { + // do nothing } ///////////////////////////////////////////////// @@ -103,8 +120,12 @@ void Thruster::Configure( ignition::gazebo::EntityComponentManager &_ecm, ignition::gazebo::EventManager &/*_eventMgr*/) { + // Create model object, to access convenient functions + auto model = ignition::gazebo::Model(_entity); + auto modelName = model.Name(_ecm); + // Get namespace - std::string ns {""}; + std::string ns = modelName; if (_sdf->HasElement("namespace")) { ns = _sdf->Get("namespace"); @@ -113,102 +134,123 @@ void Thruster::Configure( // Get joint name if (!_sdf->HasElement("joint_name")) { - ignerr << "No joint to treat as propeller found \n"; + ignerr << "Missing . Plugin won't be initialized." + << std::endl; return; } auto jointName = _sdf->Get("joint_name"); // Get thrust coefficient - if (!_sdf->HasElement("thrust_coefficient")) + if (_sdf->HasElement("thrust_coefficient")) { - ignerr << "Failed to get thrust_coefficient" << "\n"; - return; + this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); } - this->dataPtr->thrustCoefficient = _sdf->Get("thrust_coefficient"); // Get propeller diameter - if (!_sdf->HasElement("propeller_diameter")) + if (_sdf->HasElement("propeller_diameter")) { - ignerr << "Failed to get propeller_diameter \n"; + this->dataPtr->propellerDiameter = _sdf->Get("propeller_diameter"); } - this->dataPtr->propellerDiameter = _sdf->Get("propeller_diameter"); // Get fluid density, default to water otherwise if (_sdf->HasElement("fluid_density")) { this->dataPtr->fluidDensity = _sdf->Get("fluid_density"); } - igndbg << "Setting fluid density to: " << this->dataPtr->fluidDensity << "\n"; - - // Create model object, to access convenient functions - auto model = ignition::gazebo::Model(_entity); - auto jointEntity = model.JointByName(_ecm, jointName); - auto childLink = - _ecm.Component(jointEntity); + this->dataPtr->jointEntity = model.JointByName(_ecm, jointName); + if (kNullEntity == this->dataPtr->jointEntity) + { + ignerr << "Failed to find joint [" << jointName << "] in model [" + << modelName << "]. Plugin not initialized." << std::endl; + return; + } this->dataPtr->jointAxis = - _ecm.Component(jointEntity) - ->Data().Xyz(); + _ecm.Component( + this->dataPtr->jointEntity)->Data().Xyz(); - std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( + this->dataPtr->jointPose = _ecm.Component( + this->dataPtr->jointEntity)->Data(); + + // Keeping cmd_pos for backwards compatibility + // TODO(chapulina) Deprecate cmd_pos, because the commands aren't positions + std::string thrusterTopicOld = ignition::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/cmd_pos"); + this->dataPtr->node.Subscribe( + thrusterTopicOld, + &ThrusterPrivateData::OnCmdThrust, + this->dataPtr.get()); + + // Subscribe to force commands + std::string thrusterTopic = ignition::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/cmd_thrust"); + this->dataPtr->node.Subscribe( thrusterTopic, &ThrusterPrivateData::OnCmdThrust, this->dataPtr.get()); + ignmsg << "Thruster listening to commands in [" << thrusterTopic << "]" + << std::endl; + // Get link entity + auto childLink = + _ecm.Component( + this->dataPtr->jointEntity); this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data()); - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) - { - _ecm.CreateComponent(this->dataPtr->linkEntity, - ignition::gazebo::components::AngularVelocity()); - } + // Create necessary components if not present. + enableComponent(_ecm, this->dataPtr->linkEntity); + enableComponent(_ecm, + this->dataPtr->linkEntity); - // Create an angular velocity component if one is not present. - if (!_ecm.Component( - this->dataPtr->linkEntity)) + if (_sdf->HasElement("velocity_control")) { - _ecm.CreateComponent(this->dataPtr->linkEntity, - ignition::gazebo::components::WorldAngularVelocity()); + this->dataPtr->velocityControl = _sdf->Get("velocity_control"); } - double p = 0.1; - double i = 0; - double d = 0; - double iMax = 1; - double iMin = -1; - double cmdMax = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMax); - double cmdMin = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMin); - double cmdOffset = 0; - - if (_sdf->HasElement("p_gain")) - { - p = _sdf->Get("p_gain"); - } - if (!_sdf->HasElement("i_gain")) + if (!this->dataPtr->velocityControl) { - i = _sdf->Get("i_gain"); + igndbg << "Using PID controller for propeller joint." << std::endl; + + double p = 0.1; + double i = 0; + double d = 0; + double iMax = 1; + double iMin = -1; + double cmdMax = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMax); + double cmdMin = this->dataPtr->ThrustToAngularVec(this->dataPtr->cmdMin); + double cmdOffset = 0; + + if (_sdf->HasElement("p_gain")) + { + p = _sdf->Get("p_gain"); + } + if (!_sdf->HasElement("i_gain")) + { + i = _sdf->Get("i_gain"); + } + if (!_sdf->HasElement("d_gain")) + { + d = _sdf->Get("d_gain"); + } + + this->dataPtr->propellerController.Init( + p, + i, + d, + iMax, + iMin, + cmdMax, + cmdMin, + cmdOffset); } - if (!_sdf->HasElement("d_gain")) + else { - d = _sdf->Get("d_gain"); + igndbg << "Using velocity control for propeller joint." << std::endl; } - - this->dataPtr->rpmController.Init( - p, - i, - d, - iMax, - iMin, - cmdMax, - cmdMin, - cmdOffset); } ///////////////////////////////////////////////// @@ -217,6 +259,10 @@ void ThrusterPrivateData::OnCmdThrust(const ignition::msgs::Double &_msg) std::lock_guard lock(mtx); this->thrust = ignition::math::clamp(ignition::math::fixnan(_msg.data()), this->cmdMin, this->cmdMax); + + // Thrust is proportional to the Rotation Rate squared + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 + this->propellerAngVel = this->ThrustToAngularVec(this->thrust); } ///////////////////////////////////////////////// @@ -247,27 +293,54 @@ void Thruster::PreUpdate( auto pose = worldPose(this->dataPtr->linkEntity, _ecm); // TODO(arjo129): add logic for custom coordinate frame - auto unitVector = pose.Rot().RotateVector( - this->dataPtr->jointAxis.Normalize()); + // Convert joint axis to the world frame + const auto linkWorldPose = worldPose(this->dataPtr->linkEntity, _ecm); + auto jointWorldPose = linkWorldPose * this->dataPtr->jointPose; + auto unitVector = + jointWorldPose.Rot().RotateVector(this->dataPtr->jointAxis).Normalize(); double desiredThrust; + double desiredPropellerAngVel; { std::lock_guard lock(this->dataPtr->mtx); desiredThrust = this->dataPtr->thrust; + desiredPropellerAngVel = this->dataPtr->propellerAngVel; } - // Thrust is proportional to the Rotation Rate squared - // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 - auto desiredPropellerAngVel = - this->dataPtr->ThrustToAngularVec(desiredThrust); - auto currentAngular = (link.WorldAngularVelocity(_ecm))->Dot(unitVector); - auto angularError = currentAngular - desiredPropellerAngVel; + + // PID control double torque = 0.0; - if (abs(angularError) > 0.1) - torque = this->dataPtr->rpmController.Update(angularError, _info.dt); + if (!this->dataPtr->velocityControl) + { + auto currentAngular = (link.WorldAngularVelocity(_ecm))->Dot(unitVector); + auto angularError = currentAngular - desiredPropellerAngVel; + if (abs(angularError) > 0.1) + { + torque = this->dataPtr->propellerController.Update(angularError, + _info.dt); + } + } + // Velocity control + else + { + auto velocityComp = + _ecm.Component( + this->dataPtr->jointEntity); + if (velocityComp == nullptr) + { + _ecm.CreateComponent(this->dataPtr->jointEntity, + components::JointVelocityCmd({desiredPropellerAngVel})); + } + else + { + velocityComp->Data()[0] = desiredPropellerAngVel; + } + } + // Force: thrust + // Torque: propeller rotation, if using PID link.AddWorldWrench( _ecm, - unitVector * this->dataPtr->thrust, + unitVector * desiredThrust, unitVector * torque); } @@ -277,3 +350,4 @@ IGNITION_ADD_PLUGIN( Thruster::ISystemPreUpdate) IGNITION_ADD_PLUGIN_ALIAS(Thruster, "ignition::gazebo::systems::Thruster") + diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 176cc823988..ec1a607d215 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -32,45 +32,58 @@ namespace systems // Forward declaration class ThrusterPrivateData; - /// \brief This class provides a class that simulates a maritime thruster for + /// \brief This plugin simulates a maritime thruster for /// boats and underwater vehicles. It uses the equations described in Fossen's /// "Guidance and Control of Ocean Vehicles" in page 246. This plugin takes in /// force in Newtons and applies it to the thruster. It also calculates the - /// theoretical RPM of the blades and spins them at that RPM. The rationale - /// for directly using force + /// theoretical angular velocity of the blades and spins them accordingly. /// /// ## System Parameters - /// - The namespace in which the robot exists. The plugin will - /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_pos`. + /// - - The namespace in which the robot exists. The plugin will + /// listen on the topic `/model/{namespace}/joint/{joint_name}/cmd_thrust`. /// [Optional] - /// - This is the joint in the model which corresponds to the + /// - - This is the joint in the model which corresponds to the /// propeller. [Required] - /// - This is the coefficient which relates the RPM to - /// actual thrust. [Required, no units] - /// - The fluid density of the liquid in which the thruster - /// is operating in. [Required, kgm^-3] - /// - The propeller diameter is the diameter of the prop - /// in meters. [Required, m] + /// - - The fluid density of the liquid in which the thruster + /// is operating in. [Optional, kg/m^3, defaults to 1000 kg/m^3] + /// - - The diameter of the propeller in meters. + /// [Optional, m, defaults to 0.02m] + /// - - This is the coefficient which relates the angular + /// velocity to actual thrust. [Optional, no units, defaults to 1.0] /// - /// # Example - /// An example configuration is provided in the examples folder. The example + /// omega = sqrt(thrust / + /// (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + /// + /// Where omega is the propeller's angular velocity in rad/s. + /// - - If true, use joint velocity commands to rotate the + /// propeller. If false, use a PID controller to apply wrenches directly to + /// the propeller link instead. [Optional, defaults to false]. + /// - - Proportional gain for joint PID controller. [Optional, + /// no units, defaults to 0.1] + /// - - Integral gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// - - Derivative gain for joint PID controller. [Optional, + /// no units, defaults to 0.0] + /// + /// ## Example + /// An example configuration is installed with Gazebo. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the /// thruster plugin to propell the craft and the buoyancy plugin for buoyant - /// force. To run th example run. + /// force. To run the example: /// ``` /// ign gazebo auv_controls.sdf /// ``` - /// To control the rudder of the craft run the following + /// To control the rudder of the craft run the following: /// ``` /// ign topic -t /model/tethys/joint/vertical_fins_joint/0/cmd_pos /// -m ignition.msgs.Double -p 'data: -0.17' /// ``` - /// To apply a thrust you may run the following command - /// The vehicle should move in a circle. + /// To apply a thrust you may run the following command: /// ``` - /// ign topic -t /model/tethys/joint/propeller_joint/cmd_pos + /// ign topic -t /model/tethys/joint/propeller_joint/cmd_thrust /// -m ignition.msgs.Double -p 'data: -31' /// ``` + /// The vehicle should move in a circle. class Thruster: public ignition::gazebo::System, public ignition::gazebo::ISystemConfigure, @@ -79,9 +92,6 @@ namespace systems /// \brief Constructor public: Thruster(); - /// \brief Destructor - public: ~Thruster() override; - /// Documentation inherited public: void Configure( const ignition::gazebo::Entity &_entity, diff --git a/src/systems/track_controller/CMakeLists.txt b/src/systems/track_controller/CMakeLists.txt new file mode 100644 index 00000000000..a1f7ac1df70 --- /dev/null +++ b/src/systems/track_controller/CMakeLists.txt @@ -0,0 +1,11 @@ +gz_add_system(track-controller + SOURCES + TrackController.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-physics${IGN_PHYSICS_VER}::ignition-physics${IGN_PHYSICS_VER} + ignition-msgs${IGN_MSGS_VER}::ignition-msgs${IGN_MSGS_VER} + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_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/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc new file mode 100644 index 00000000000..116d3cb7876 --- /dev/null +++ b/src/systems/track_controller/TrackController.cc @@ -0,0 +1,622 @@ +/* + * 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 "TrackController.hh" + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include "ignition/gazebo/components/Collision.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/ParentEntity.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +class ignition::gazebo::systems::TrackControllerPrivate +{ + public : ~TrackControllerPrivate() {} + /// \brief Register a collision entity to work with this system (e.g. enable + /// custom collision processing). + /// \param[in] _ecm Entity Component Manager + /// \param[in] _entity The collision to register + /// \param[in] _link The link to which the collision belongs + public: void RegisterCollision(EntityComponentManager& _ecm, + const Entity& _entity, const Entity& _link); + + /// \brief Set velocity command to the track. + /// \param[in] _msg The command. + public: void OnCmdVel(const msgs::Double& _msg); + + /// \brief Set center of rotation command to the track. + /// \param[in] _msg The command. + public: void OnCenterOfRotation(const msgs::Vector3d& _msg); + + public: using P = physics::FeaturePolicy3d; + public: using F = physics::SetContactPropertiesCallbackFeature; + + /// \brief The callback for CollectContactSurfaceProperties - all the magic + /// happens here. + /// \param[in] _collision1 The first colliding body. + /// \param[in] _collision2 The second colliding body. + /// \param[in] _point The contact point (in world coords). + /// \param[in] _force Force in the contact point (may be omitted). + /// \param[in] _normal Unit normal of the collision. + /// \param[in] _depth Depth of penetration. + /// \param[in] _numContactsOnCollision Number of contacts that share the same + /// collision body. + /// \param[inout] _params The contact surface parameters to be set by this + /// system. + public: void ComputeSurfaceProperties( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional& _normal, + F::ContactSurfaceParams

& _params); + + /// \brief Compute speed and direction of motion of the contact surface. + /// \param[in] _beltSpeed Speed of the belt. + /// \param[in] _beltDirection Direction of the belt (in world coords). + /// \param[in] _frictionDirection First friction direction (in world coords). + /// \return The computed contact surface speed. + public: double ComputeSurfaceMotion( + double _beltSpeed, const ignition::math::Vector3d &_beltDirection, + const ignition::math::Vector3d &_frictionDirection); + + /// \brief Compute the first friction direction of the contact surface. + /// \param[in] _centerOfRotation The point around which the track circles ( + /// +Inf vector in case of straight motion). + /// \param[in] _contactWorldPosition Position of the contact point. + /// \param[in] _contactNormal Normal of the contact surface (in world coords). + /// \param[in] _beltDirection Direction of the belt (in world coords). + public: ignition::math::Vector3d ComputeFrictionDirection( + const ignition::math::Vector3d &_centerOfRotation, + const ignition::math::Vector3d &_contactWorldPosition, + const ignition::math::Vector3d &_contactNormal, + const ignition::math::Vector3d &_beltDirection); + + /// \brief Name of the link to which the track is attached. + public: std::string linkName; + + /// \brief Orientation of the track relative to the link. It is assumed that + /// the track moves along the +x direction of the transformed coordinate + /// system. + public: math::Quaterniond trackOrientation; + + /// \brief Enables debugging prints and visualizations. + public: bool debug {false}; + /// \brief Cached marker message for debugging purposes. + public: msgs::Marker debugMarker; + /// \brief ID of the debug marker. Should reset to 0 at each iteration start. + public: uint64_t markerId; + + /// \brief Event manager. + public: EventManager* eventManager; + /// \brief Connection to CollectContactSurfaceProperties event. + public: common::ConnectionPtr eventConnection; + /// \brief Ignition transport node. + public: transport::Node node; + + /// \brief The model this plugin is attached to. + public: Model model; + /// \brief Entity of the link this track is attached to. + public: Entity linkEntity {kNullEntity}; + /// \brief Entities of all collision elements of the track's link. + public: std::unordered_set trackCollisions; + + /// \brief World pose of the track's link. + public: math::Pose3d linkWorldPose; + /// \brief World poses of all collision elements of the track's link. + public: std::unordered_map collisionsWorldPose; + + /// \brief The last commanded velocity. + public: double velocity {0}; + /// \brief Commanded velocity clipped to allowable range. + public: double limitedVelocity {0}; + /// \brief Previous clipped commanded velocity. + public: double prevVelocity {0}; + /// \brief Second previous clipped commanded velocity. + public: double prevPrevVelocity {0}; + /// \brief The point around which the track circles (in world coords). Should + /// be set to +Inf if the track is going straight. + public: math::Vector3d centerOfRotation {math::Vector3d::Zero * math::INF_D}; + /// \brief protects velocity and centerOfRotation + public: std::mutex cmdMutex; + + /// \brief Maximum age of a command in seconds. If a command is older, the + /// track automatically sets a zero velocity. Set this to max() to denote + /// commands do not time out. + public: std::chrono::steady_clock::duration maxCommandAge + {std::chrono::steady_clock::duration::max()}; + + /// \brief This variable is set to true each time a new command arrives. + /// It is intended to be set to false after the command is processed. + public: bool hasNewCommand{false}; + + /// \brief The time at which the last command has been received. + public: std::chrono::steady_clock::duration lastCommandTime; + + /// \brief Limiter of the commanded velocity. + public: math::SpeedLimiter limiter; +}; + +////////////////////////////////////////////////// +TrackController::TrackController() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +TrackController::~TrackController() +{ +} + +////////////////////////////////////////////////// +void TrackController::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) +{ + this->dataPtr->eventManager = &_eventMgr; + + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "TrackController should be attached to a model " + << "entity. Failed to initialize." << std::endl; + return; + } + + if (!_sdf->HasElement("link")) + { + ignerr << "TrackController plugin is missing element." << std::endl; + return; + } + this->dataPtr->linkName = _sdf->Get("link"); + + using P = physics::FeaturePolicy3d; + using F = physics::SetContactPropertiesCallbackFeature; + + this->dataPtr->eventConnection = this->dataPtr->eventManager-> + Connect( + [this]( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional /* _force */, + const std::optional _normal, + const std::optional /* _depth */, + const size_t /*_numContactsOnCollision*/, + F::ContactSurfaceParams

& _params) + { + this->dataPtr->ComputeSurfaceProperties(_collision1, _collision2, + _point, _normal, _params); + } + ); + + _ecm.Each( + [&](const Entity & _collisionEntity, + const components::Collision */*_collision*/, + const components::Name */*_name*/, + const components::ParentEntity *_parent) + { + this->dataPtr->RegisterCollision(_ecm, _collisionEntity, _parent->Data()); + return true; + } + ); + + const auto topicPrefix = "/model/" + this->dataPtr->model.Name(_ecm) + + "/link/" + this->dataPtr->linkName; + + const auto kDefaultVelTopic = topicPrefix + "/track_cmd_vel"; + const auto velTopic = validTopic({_sdf->Get( + "velocity_topic", kDefaultVelTopic).first, kDefaultVelTopic}); + if (!this->dataPtr->node.Subscribe( + velTopic, &TrackControllerPrivate::OnCmdVel, this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << velTopic << "]. " + << "Track will not receive commands." << std::endl; + return; + } + igndbg << "Subscribed to " << velTopic << " for receiving track velocity " + << "commands." << std::endl; + + const auto kDefaultCorTopic = topicPrefix + "/track_cmd_center_of_rotation"; + const auto corTopic = validTopic({_sdf->Get( + "center_of_rotation_topic", kDefaultCorTopic).first, kDefaultCorTopic}); + if (!this->dataPtr->node.Subscribe( + corTopic, &TrackControllerPrivate::OnCenterOfRotation, + this->dataPtr.get())) + { + ignerr << "Error subscribing to topic [" << corTopic << "]. " + << "Track will not receive center of rotation commands." + << std::endl; + return; + } + igndbg << "Subscribed to " << corTopic << " for receiving track center " + << "of rotation commands." << std::endl; + + this->dataPtr->trackOrientation = _sdf->Get( + "track_orientation", math::Quaterniond::Identity).first; + + if (_sdf->HasElement("max_command_age")) + { + const auto seconds = _sdf->Get("max_command_age"); + this->dataPtr->maxCommandAge = + std::chrono::duration_cast( + std::chrono::duration(seconds)); + igndbg << "Track commands will time out after " << seconds << " seconds" + << std::endl; + } + + auto hasVelocityLimits = false; + auto hasAccelerationLimits = false; + auto hasJerkLimits = false; + auto minVel = std::numeric_limits::lowest(); + auto maxVel = std::numeric_limits::max(); + auto minAccel = std::numeric_limits::lowest(); + auto maxAccel = std::numeric_limits::max(); + auto minJerk = std::numeric_limits::lowest(); + auto maxJerk = std::numeric_limits::max(); + + if (_sdf->HasElement("min_velocity")) + { + minVel = _sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("max_velocity")) + { + maxVel = _sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (_sdf->HasElement("min_acceleration")) + { + minAccel = _sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("max_acceleration")) + { + maxAccel = _sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (_sdf->HasElement("min_jerk")) + { + minJerk = _sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (_sdf->HasElement("max_jerk")) + { + maxJerk = _sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + if (hasVelocityLimits) + { + this->dataPtr->limiter.SetMinVelocity(minVel); + this->dataPtr->limiter.SetMaxVelocity(maxVel); + } + if (hasAccelerationLimits) + { + this->dataPtr->limiter.SetMinAcceleration(minAccel); + this->dataPtr->limiter.SetMaxAcceleration(maxAccel); + } + if (hasJerkLimits) + { + this->dataPtr->limiter.SetMinJerk(minJerk); + this->dataPtr->limiter.SetMaxJerk(maxJerk); + } + + this->dataPtr->debug = _sdf->Get("debug", false).first; + if (this->dataPtr->debug) + { + this->dataPtr->debugMarker.set_ns(this->dataPtr->linkName + "/friction"); + this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::BOX); + this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); + this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); + + // Set material properties + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + // Set marker scale + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_scale(), + ignition::math::Vector3d(0.3, 0.03, 0.03)); + } +} + +////////////////////////////////////////////////// +void TrackController::PreUpdate( + const UpdateInfo& _info, EntityComponentManager& _ecm) +{ + _ecm.EachNew( + [&](const Entity & _entity, + const components::Collision */*_collision*/, + const components::Name */*_name*/, + const components::ParentEntity *_parent) + { + this->dataPtr->RegisterCollision(_ecm, _entity, _parent->Data()); + return true; + } + ); + + // Find link entity + if (this->dataPtr->linkEntity == kNullEntity) + { + this->dataPtr->linkEntity = this->dataPtr->model.LinkByName(_ecm, + this->dataPtr->linkName); + } + if (this->dataPtr->linkEntity == kNullEntity) + { + ignwarn << "Could not find track link [" << this->dataPtr->linkName << "]" + << std::endl; + return; + } + + // Cache poses + this->dataPtr->linkWorldPose = worldPose(this->dataPtr->linkEntity, _ecm); + for (auto& collisionEntity : this->dataPtr->trackCollisions) + this->dataPtr->collisionsWorldPose[collisionEntity] = + worldPose(collisionEntity, _ecm); + + std::chrono::steady_clock::duration lastCommandTimeCopy; + { + std::lock_guard lock(this->dataPtr->cmdMutex); + if (this->dataPtr->hasNewCommand) + { + this->dataPtr->lastCommandTime = _info.simTime; + this->dataPtr->hasNewCommand = false; + } + lastCommandTimeCopy = this->dataPtr->lastCommandTime; + + // Compute limited velocity command + this->dataPtr->limitedVelocity = this->dataPtr->velocity; + } + + if (this->dataPtr->maxCommandAge != std::chrono::steady_clock::duration::max() + && (_info.simTime - lastCommandTimeCopy) > this->dataPtr->maxCommandAge) + { + this->dataPtr->limitedVelocity = 0; + } + + this->dataPtr->limiter.Limit( + this->dataPtr->limitedVelocity, // in-out parameter + this->dataPtr->prevVelocity, + this->dataPtr->prevPrevVelocity, _info.dt); + + this->dataPtr->prevPrevVelocity = this->dataPtr->prevVelocity; + this->dataPtr->prevVelocity = this->dataPtr->limitedVelocity; + + if (this->dataPtr->debug) + { + // Reset debug marker ID + this->dataPtr->markerId = 1; + } +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::ComputeSurfaceProperties( + const Entity& _collision1, + const Entity& _collision2, + const math::Vector3d& _point, + const std::optional& _normal, + F::ContactSurfaceParams

& _params + ) +{ + using math::eigen3::convert; + + if (!_normal) + { + static bool informed = false; + if (!informed) + { + ignerr << "TrackController requires a physics engine that computes " + << "contact normals!" << std::endl; + informed = true; + } + return; + } + + const auto isCollision1Track = this->trackCollisions.find(_collision1) != + this->trackCollisions.end(); + const auto isCollision2Track = this->trackCollisions.find(_collision2) != + this->trackCollisions.end(); + if (!isCollision1Track && !isCollision2Track) + return; + + const auto trackCollision = isCollision1Track ? _collision1 : _collision2; + + auto contactNormal = _normal.value(); + + // In case we have not yet cached the collision pose, skip this iteration + if (this->collisionsWorldPose.find(trackCollision) == + this->collisionsWorldPose.end()) + return; + const auto& collisionPose = this->collisionsWorldPose[trackCollision]; + + // Flip the contact normal if it points outside the track collision + if (contactNormal.Dot(collisionPose.Pos() - _point) < 0) + contactNormal = -contactNormal; + + const auto trackWorldRot = this->linkWorldPose.Rot() * this->trackOrientation; + const auto trackYAxisGlobal = + trackWorldRot.RotateVector(math::Vector3d::UnitY); + + // Vector tangent to the belt pointing in the belt's movement direction + // The belt's bottom moves backwards when the robot should move forward! + auto beltDirection = contactNormal.Cross(trackYAxisGlobal); + + if (this->limitedVelocity < 0) + beltDirection = -beltDirection; + + math::Vector3d cor; + { + std::lock_guard lock(this->cmdMutex); + cor = this->centerOfRotation; + } + + const auto frictionDirection = this->ComputeFrictionDirection( + cor, _point, contactNormal, beltDirection); + + _params.firstFrictionalDirection = + convert(isCollision1Track ? frictionDirection : -frictionDirection); + + const auto surfaceMotion = this->ComputeSurfaceMotion( + this->limitedVelocity, beltDirection, frictionDirection); + + if (!_params.contactSurfaceMotionVelocity) + _params.contactSurfaceMotionVelocity.emplace(Eigen::Vector3d::Zero()); + _params.contactSurfaceMotionVelocity->y() = surfaceMotion; + + if (this->debug) + { + igndbg << "Link: " << linkName << std::endl; + igndbg << "- is collision 1 track " << (isCollision1Track ? "1" : "0") + << std::endl; + igndbg << "- velocity cmd " << this->velocity << std::endl; + igndbg << "- limited velocity cmd " << this->limitedVelocity << std::endl; + igndbg << "- friction direction " << frictionDirection << std::endl; + igndbg << "- surface motion " << surfaceMotion << std::endl; + igndbg << "- contact point " << convert(_point) << std::endl; + igndbg << "- contact normal " << contactNormal << std::endl; + igndbg << "- track rot " << trackWorldRot << std::endl; + igndbg << "- track Y " << trackYAxisGlobal << std::endl; + igndbg << "- belt direction " << beltDirection << std::endl; + + this->debugMarker.set_id(++this->markerId); + + math::Quaterniond rot; + rot.From2Axes(math::Vector3d::UnitX, frictionDirection); + math::Vector3d p = _point; + p += rot.RotateVector( + math::Vector3d::UnitX * this->debugMarker.scale().x() / 2); + + ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + p.X(), p.Y(), p.Z(), rot.Roll(), rot.Pitch(), rot.Yaw())); + this->debugMarker.mutable_material()->mutable_diffuse()->set_r( + surfaceMotion >= 0 ? 0 : 1); + + this->node.Request("/marker", this->debugMarker); + } +} + +////////////////////////////////////////////////// +double TrackControllerPrivate::ComputeSurfaceMotion( + const double _beltSpeed, const ignition::math::Vector3d &_beltDirection, + const ignition::math::Vector3d &_frictionDirection) +{ + // the dot product is the cosine of the angle they + // form (because both are unit vectors) + // the belt should actually move in the opposite direction than is the desired + // motion of the whole track - that's why the value is negated + return -math::signum(_beltDirection.Dot(_frictionDirection)) * + fabs(_beltSpeed); +} + +////////////////////////////////////////////////// +ignition::math::Vector3d TrackControllerPrivate::ComputeFrictionDirection( + const ignition::math::Vector3d &_centerOfRotation, + const ignition::math::Vector3d &_contactWorldPosition, + const ignition::math::Vector3d &_contactNormal, + const ignition::math::Vector3d &_beltDirection) +{ + if (_centerOfRotation.IsFinite()) + { + // non-straight drive + + // vector pointing from the center of rotation to the contact point + const auto corToContact = + (_contactWorldPosition - _centerOfRotation).Normalize(); + + // the friction force should be perpendicular to corToContact + auto frictionDirection = _contactNormal.Cross(corToContact); + if (this->limitedVelocity < 0) + frictionDirection = - frictionDirection; + + return frictionDirection; + } + else + { + // straight drive + return _beltDirection; + } +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::RegisterCollision(EntityComponentManager& _ecm, + const Entity& _entity, const Entity& _link) +{ + if (this->linkEntity == kNullEntity) + this->linkEntity = this->model.LinkByName(_ecm, this->linkName); + + if (_link != this->linkEntity) + return; + + this->trackCollisions.insert(_entity); + + _ecm.SetComponentData( + _entity, true); +} + +////////////////////////////////////////////////// +void TrackControllerPrivate::OnCmdVel(const msgs::Double& _msg) +{ + std::lock_guard lock(this->cmdMutex); + this->velocity = _msg.data(); + this->hasNewCommand = true; +} + +///////////////////////////////////////////////// +void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) +{ + std::lock_guard lock(this->cmdMutex); + this->centerOfRotation = msgs::Convert(_msg); + this->hasNewCommand = true; +} + +IGNITION_ADD_PLUGIN(TrackController, + ignition::gazebo::System, + TrackController::ISystemConfigure, + TrackController::ISystemPreUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrackController, + "ignition::gazebo::systems::TrackController") diff --git a/src/systems/track_controller/TrackController.hh b/src/systems/track_controller/TrackController.hh new file mode 100644 index 00000000000..f9d54a32044 --- /dev/null +++ b/src/systems/track_controller/TrackController.hh @@ -0,0 +1,126 @@ +/* + * 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_TRACKCONTROLLER_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRACKCONTROLLER_HH_ + +#include +#include +#include "ignition/gazebo/physics/Events.hh" + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrackControllerPrivate; + + /// \brief Controller of a track on either a conveyor belt or a tracked + /// vehicle. The system should be attached to a model. If implementing a + /// tracked vehicle, use also TrackedVehicle system. + /// + /// The system is implemented along the lines of M. Pecka, K. Zimmermann and + /// T. Svoboda, "Fast simulation of vehicles with non-deformable tracks," + /// 2017 IEEE/RSJ International Conference on Intelligent Robots and + /// Systems (IROS), 2017, pp. 6414-6419, doi: 10.1109/IROS.2017.8206546. It + /// does not provide 100% plausible track drivetrain simulation, but provides + /// one that is provably better than a set of wheels instead of the track. + /// Only velocity control is supported - no effort controller is available. + /// The basic idea of the implementation is utilizing the so called "contact + /// surface motion" parameter of each contact point between the track and the + /// environment. Instead of telling the physics engine to push velocity + /// towards zero in contact points (up to friction), it tells it to maintain + /// the desired track velocity in the contact point (up to friction). For + /// better behavior when turning with tracked vehicles, it also accepts the + /// position of the center of rotation of the whole vehicle so that it can + /// adjust the direction of friction along the desired circle. This system + /// does not simulate the effect of grousers. The best way to achieve a + /// similar effect is to set a very high `` for the track links. + /// + /// # Examples + /// + /// See example usage in worlds example/conveyor.sdf and + /// example/tracked_vehicle_simple.sdf . + /// + /// # System Parameters + /// + /// `` Name of the link the controller controls. Required parameter. + /// + /// `` If 1, the system will output debugging info and visualizations. + /// The default value is 0. + /// + /// `` Orientation of the track relative to the link. + /// It is assumed that the track moves along the +x direction of the + /// transformed coordinate system. Defaults to no rotation (`0 0 0`). + /// + /// `` Name of the topic on which the system accepts velocity + /// commands. + /// Defaults to `/model/${model_name}/link/${link_name}/track_cmd_vel`. + /// + /// `` The topic on which the track accepts center + /// of rotation commands. Defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_center_of_rotation`. + /// + /// `` If this parameter is set, each velocity or center of + /// rotation command will only act for the given number of seconds and the + /// track will be stopped if no command arrives before this timeout. + /// + /// ``/`` Min/max velocity of the track (m/s). + /// If not specified, the velocity is not limited (however the physics will, + /// in the end, have some implicit limit). + /// + /// ``/`` Min/max acceleration of the + /// track (m/s^2). If not specified, the acceleration is not limited + /// (however the physics will, in the end, have some implicit limit). + /// + /// ``/`` Min/max jerk of the track (m/s^3). If not + /// specified, the acceleration is not limited (however the physics will, + /// in the end, have some implicit limit). + class TrackController + : public System, + public ISystemConfigure, + public ISystemPreUpdate + { + /// \brief Constructor + public: TrackController(); + + /// \brief Destructor + public: ~TrackController() override; + + // 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/tracked_vehicle/CMakeLists.txt b/src/systems/tracked_vehicle/CMakeLists.txt new file mode 100644 index 00000000000..8ee3b43f1ac --- /dev/null +++ b/src/systems/tracked_vehicle/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(tracked-vehicle + SOURCES + TrackedVehicle.cc + PUBLIC_LINK_LIBS + ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} + ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER} + ignition-transport${IGN_TRANSPORT_VER}::ignition-transport${IGN_TRANSPORT_VER} +) diff --git a/src/systems/tracked_vehicle/TrackedVehicle.cc b/src/systems/tracked_vehicle/TrackedVehicle.cc new file mode 100644 index 00000000000..e175d44329c --- /dev/null +++ b/src/systems/tracked_vehicle/TrackedVehicle.cc @@ -0,0 +1,778 @@ +/* + * 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 "TrackedVehicle.hh" + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/CanonicalLink.hh" +#include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Util.hh" + +using namespace ignition; +using namespace gazebo; +using namespace systems; + +/// \brief Velocity command. +struct Commands +{ + /// \brief Linear velocity. + double lin {0.0}; + + /// \brief Angular velocity. + double ang {0.0}; + + Commands() {} +}; + +class ignition::gazebo::systems::TrackedVehiclePrivate +{ + /// \brief Callback for velocity subscription + /// \param[in] _msg Velocity message + public: void OnCmdVel(const ignition::msgs::Twist &_msg); + + /// \brief Callback for steering efficiency subscription + /// \param[in] _msg Steering efficiency message + public: void OnSteeringEfficiency(const ignition::msgs::Double &_msg); + + /// \brief Update odometry and publish an odometry message. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateOdometry(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Update the linear and angular velocities. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateVelocity(const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm); + + /// \brief Ignition communication node. + public: transport::Node node; + + /// \brief The link of the vehicle body (should be between left and right + /// tracks, center of this link will be the center of rotation). + public: Entity bodyLink {kNullEntity}; + + /// \brief Entities of the left tracks + public: std::vector leftTracks; + + /// \brief Entities of the right tracks + public: std::vector rightTracks; + + /// \brief Name of the body link + public: std::string bodyLinkName; + + /// \brief Names of left tracks + public: std::vector leftTrackNames; + + /// \brief Names of right tracks + public: std::vector rightTrackNames; + + /// \brief Velocity publishers of tracks. + public: std::unordered_map + velPublishers; + + /// \brief Center of rotation publishers of tracks. + public: std::unordered_map + corPublishers; + + /// \brief Calculated speed of left tracks + public: double leftSpeed{0}; + + /// \brief Calculated speed of right tracks + public: double rightSpeed{0}; + + /// \brief Radius of the desired rotation (rad). + public: double desiredRotationRadiusSigned {0}; + + /// \brief Fake position encoder of left track (for computing odometry). + public: math::Angle odomLeftWheelPos {0}; + + /// \brief Fake position encoder of left track (for computing odometry). + public: math::Angle odomRightWheelPos {0}; + + /// \brief The point around which the vehicle should circle (in world coords). + public: math::Vector3d centerOfRotation {0, 0, 0}; + + /// \brief Distance between tracks. + public: double tracksSeparation{1.0}; + + /// \brief Height of the tracks. + public: double trackHeight{0.2}; + + /// \brief Steering efficiency. + public: double steeringEfficiency{0.5}; + + /// \brief Model interface + public: Model model{kNullEntity}; + + /// \brief The model's canonical link. + public: Link canonicalLink{kNullEntity}; + + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odomPubPeriod{0}; + + /// \brief Last sim time odom was published. + public: std::chrono::steady_clock::duration lastOdomPubTime{0}; + + /// \brief Diff drive odometry. + public: math::DiffDriveOdometry odom; + + /// \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; + + /// \brief Angular velocity limiter. + public: std::unique_ptr limiterAng; + + /// \brief Previous control command. + public: Commands last0Cmd; + + /// \brief Previous control command to last0Cmd. + public: Commands last1Cmd; + + /// \brief Last target velocity requested. + public: msgs::Twist targetVel; + + /// \brief This variable is set to true each time a new command arrives. + /// It is intended to be set to false after the command is processed. + public: bool hasNewCommand{false}; + + /// \brief A mutex to protect the target velocity command. + public: std::mutex mutex; + + /// \brief frame_id from sdf. + public: std::string sdfFrameId; + + /// \brief child_frame_id from sdf. + public: std::string sdfChildFrameId; + + /// \brief Enables debugging prints and visualizations. + public: bool debug {false}; + + /// \brief Cached marker message for debugging purposes. + public: msgs::Marker debugMarker; +}; + +////////////////////////////////////////////////// +TrackedVehicle::TrackedVehicle() + : dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +TrackedVehicle::~TrackedVehicle() +{ +} + +////////////////////////////////////////////////// +void TrackedVehicle::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) +{ + this->dataPtr->model = Model(_entity); + + if (!this->dataPtr->model.Valid(_ecm)) + { + ignerr << "TrackedVehicle plugin should be attached to a model entity. " + << "Failed to initialize." << std::endl; + return; + } + + const auto& modelName = this->dataPtr->model.Name(_ecm); + + // Get the canonical link + std::vector links = _ecm.ChildrenByComponents( + _entity, components::CanonicalLink()); + if (!links.empty()) + this->dataPtr->canonicalLink = Link(links[0]); + + // Ugly, but needed because the sdf::Element::GetElement is not a const + // function and _sdf is a const shared pointer to a const sdf::Element. + auto ptr = const_cast(_sdf.get()); + + std::unordered_map tracks; + + if (_sdf->HasElement("body_link")) + this->dataPtr->bodyLinkName = _sdf->Get("body_link"); + + // Get params from SDF + sdf::ElementPtr sdfElem = ptr->GetElement("left_track"); + while (sdfElem) + { + const auto& linkName = sdfElem->Get("link"); + this->dataPtr->leftTrackNames.push_back(linkName); + tracks[linkName] = sdfElem; + sdfElem = sdfElem->GetNextElement("left_track"); + } + sdfElem = ptr->GetElement("right_track"); + while (sdfElem) + { + const auto& linkName = sdfElem->Get("link"); + this->dataPtr->rightTrackNames.push_back(linkName); + tracks[linkName] = sdfElem; + sdfElem = sdfElem->GetNextElement("right_track"); + } + + for (const auto &[linkName, elem] : tracks) + { + const auto prefix = "/model/" + modelName + "/link/" + linkName; + + auto topic = validTopic({elem->Get( + "velocity_topic", prefix + "/track_cmd_vel").first}); + this->dataPtr->velPublishers[linkName] = + this->dataPtr->node.Advertise(topic); + + topic = validTopic({elem->Get("center_of_rotation_topic", + prefix + "/track_cmd_center_of_rotation").first}); + this->dataPtr->corPublishers[linkName] = + this->dataPtr->node.Advertise(topic); + } + + this->dataPtr->tracksSeparation = _sdf->Get("tracks_separation", + this->dataPtr->tracksSeparation).first; + this->dataPtr->steeringEfficiency = _sdf->Get("steering_efficiency", + this->dataPtr->steeringEfficiency).first; + + // Instantiate the speed limiters. + this->dataPtr->limiterLin = std::make_unique(); + this->dataPtr->limiterAng = std::make_unique(); + + std::map limits = { + {"linear_velocity", this->dataPtr->limiterLin.get()}, + {"angular_velocity", this->dataPtr->limiterAng.get()}, + }; + + for (auto& [tag, limiter] : limits) + { + if (!_sdf->HasElement(tag)) + continue; + + auto sdf = ptr->GetElement(tag); + + // Parse speed limiter parameters. + bool hasVelocityLimits = false; + bool hasAccelerationLimits = false; + bool hasJerkLimits = false; + double minVel = std::numeric_limits::lowest(); + double maxVel = std::numeric_limits::max(); + double minAccel = std::numeric_limits::lowest(); + double maxAccel = std::numeric_limits::max(); + double minJerk = std::numeric_limits::lowest(); + double maxJerk = std::numeric_limits::max(); + + if (sdf->HasElement("min_velocity")) + { + minVel = sdf->Get("min_velocity"); + hasVelocityLimits = true; + } + if (sdf->HasElement("max_velocity")) + { + maxVel = sdf->Get("max_velocity"); + hasVelocityLimits = true; + } + if (sdf->HasElement("min_acceleration")) + { + minAccel = sdf->Get("min_acceleration"); + hasAccelerationLimits = true; + } + if (sdf->HasElement("max_acceleration")) + { + maxAccel = sdf->Get("max_acceleration"); + hasAccelerationLimits = true; + } + if (sdf->HasElement("min_jerk")) + { + minJerk = sdf->Get("min_jerk"); + hasJerkLimits = true; + } + if (sdf->HasElement("max_jerk")) + { + maxJerk = sdf->Get("max_jerk"); + hasJerkLimits = true; + } + + if (hasVelocityLimits) + { + limiter->SetMinVelocity(minVel); + limiter->SetMaxVelocity(maxVel); + } + + if (hasAccelerationLimits) + { + limiter->SetMinAcceleration(minAccel); + limiter->SetMaxAcceleration(maxAccel); + } + + if (hasJerkLimits) + { + limiter->SetMinJerk(minJerk); + limiter->SetMaxJerk(maxJerk); + } + } + + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration odomPer{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast(odomPer); + } + + // Setup odometry. + this->dataPtr->odom.SetWheelParams(this->dataPtr->tracksSeparation, + this->dataPtr->trackHeight/2, this->dataPtr->trackHeight/2); + + // Subscribe to commands + const auto topicPrefix = "/model/" + this->dataPtr->model.Name(_ecm); + + const auto kDefaultCmdVelTopic {topicPrefix + "/cmd_vel"}; + const auto topic = validTopic({ + _sdf->Get("topic", kDefaultCmdVelTopic).first, + kDefaultCmdVelTopic}); + + this->dataPtr->node.Subscribe(topic, &TrackedVehiclePrivate::OnCmdVel, + this->dataPtr.get()); + + const auto kDefaultOdomTopic {topicPrefix + "/odometry"}; + const auto odomTopic = validTopic({ + _sdf->Get("odom_topic", kDefaultOdomTopic).first, + kDefaultOdomTopic}); + + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopic); + + const auto kDefaultTfTopic {topicPrefix + "/tf"}; + const auto tfTopic = validTopic({ + _sdf->Get("tf_topic", kDefaultTfTopic).first, + kDefaultTfTopic}); + + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); + + const auto kDefaultSeTopic {topicPrefix + "/steering_efficiency"}; + const auto seTopic = validTopic({ + _sdf->Get("steering_efficiency_topic", kDefaultSeTopic).first, + kDefaultSeTopic}); + + this->dataPtr->node.Subscribe(seTopic, + &TrackedVehiclePrivate::OnSteeringEfficiency, this->dataPtr.get()); + + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); + + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + + ignmsg << "TrackedVehicle [" << modelName << "] loaded:" << std::endl; + ignmsg << "- tracks separation: " << this->dataPtr->tracksSeparation + << " m" << std::endl; + ignmsg << "- track height (for odometry): " << this->dataPtr->trackHeight + << " m" << std::endl; + ignmsg << "- initial steering efficiency: " + << this->dataPtr->steeringEfficiency << std::endl; + ignmsg << "- subscribing to twist messages on [" << topic << "]" << std::endl; + ignmsg << "- subscribing to steering efficiency messages on [" + << seTopic << "]" << std::endl; + ignmsg << "- publishing odometry on [" << odomTopic << "]" << std::endl; + ignmsg << "- publishing TF on [" << tfTopic << "]" << std::endl; + + // Initialize debugging helpers if needed + this->dataPtr->debug = _sdf->Get("debug", false).first; + if (this->dataPtr->debug) + { + this->dataPtr->debugMarker.set_ns( + this->dataPtr->model.Name(_ecm) + "/cor"); + this->dataPtr->debugMarker.set_action(ignition::msgs::Marker::ADD_MODIFY); + this->dataPtr->debugMarker.set_type(ignition::msgs::Marker::SPHERE); + this->dataPtr->debugMarker.set_visibility(ignition::msgs::Marker::GUI); + this->dataPtr->debugMarker.mutable_lifetime()->set_sec(0); + this->dataPtr->debugMarker.mutable_lifetime()->set_nsec(4000000); + this->dataPtr->debugMarker.set_id(1); + + // Set material properties + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_ambient(), + ignition::math::Color(0, 0, 1, 1)); + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_material()->mutable_diffuse(), + ignition::math::Color(0, 0, 1, 1)); + + // Set marker scale + ignition::msgs::Set( + this->dataPtr->debugMarker.mutable_scale(), + ignition::math::Vector3d(0.1, 0.1, 0.1)); + } +} + +////////////////////////////////////////////////// +void TrackedVehicle::PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::PreUpdate"); + + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + ignwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. Resetting odometry." << std::endl; + this->dataPtr->odom.Init( + std::chrono::steady_clock::time_point(_info.simTime)); + } + + // If the links haven't been identified yet, look for them + static std::set warnedModels; + auto modelName = this->dataPtr->model.Name(_ecm); + + if (this->dataPtr->bodyLink == kNullEntity) + { + if (!this->dataPtr->bodyLinkName.empty()) + this->dataPtr->bodyLink = + this->dataPtr->model.LinkByName(_ecm, this->dataPtr->bodyLinkName); + else + this->dataPtr->bodyLink = this->dataPtr->canonicalLink.Entity(); + + if (this->dataPtr->bodyLink == kNullEntity) + { + static bool warned {false}; + if (!warned) + { + ignwarn << "Failed to find body link [" << this->dataPtr->bodyLinkName + << "] for model [" << modelName << "]" << std::endl; + warned = true; + } + return; + } + } + + if (this->dataPtr->leftTracks.empty() || + this->dataPtr->rightTracks.empty()) + { + bool warned{false}; + for (const std::string &name : this->dataPtr->leftTrackNames) + { + Entity track = this->dataPtr->model.LinkByName(_ecm, name); + if (track != kNullEntity) + this->dataPtr->leftTracks.push_back(track); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find left track [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + + for (const std::string &name : this->dataPtr->rightTrackNames) + { + Entity track = this->dataPtr->model.LinkByName(_ecm, name); + if (track != kNullEntity) + this->dataPtr->rightTracks.push_back(track); + else if (warnedModels.find(modelName) == warnedModels.end()) + { + ignwarn << "Failed to find right track [" << name << "] for model [" + << modelName << "]" << std::endl; + warned = true; + } + } + if (warned) + { + warnedModels.insert(modelName); + } + } + + if (this->dataPtr->leftTracks.empty() || this->dataPtr->rightTracks.empty()) + return; + + if (warnedModels.find(modelName) != warnedModels.end()) + { + ignmsg << "Found tracks for model [" << modelName + << "], plugin will start working." << std::endl; + warnedModels.erase(modelName); + } +} + +////////////////////////////////////////////////// +void TrackedVehicle::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::PostUpdate"); + // Nothing left to do if paused. + if (_info.paused) + return; + + this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::UpdateOdometry( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::UpdateOdometry"); + // Initialize, if not already initialized. + if (!this->odom.Initialized()) + { + this->odom.Init(std::chrono::steady_clock::time_point(_info.simTime)); + return; + } + + if (this->leftTracks.empty() || this->rightTracks.empty()) + return; + + this->odom.Update(this->odomLeftWheelPos, this->odomRightWheelPos, + std::chrono::steady_clock::time_point(_info.simTime)); + + // Throttle publishing + auto diff = _info.simTime - this->lastOdomPubTime; + if (diff > std::chrono::steady_clock::duration::zero() && + diff < this->odomPubPeriod) + { + return; + } + this->lastOdomPubTime = _info.simTime; + + // Construct the odometry message and publish it. + msgs::Odometry msg; + msg.mutable_pose()->mutable_position()->set_x(this->odom.X()); + msg.mutable_pose()->mutable_position()->set_y(this->odom.Y()); + + math::Quaterniond orientation(0, 0, *this->odom.Heading()); + msgs::Set(msg.mutable_pose()->mutable_orientation(), orientation); + + msg.mutable_twist()->mutable_linear()->set_x(this->odom.LinearVelocity()); + msg.mutable_twist()->mutable_angular()->set_z(*this->odom.AngularVelocity()); + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set the frame id. + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + if (this->sdfFrameId.empty()) + { + frame->add_value(this->model.Name(_ecm) + "/odom"); + } + else + { + frame->add_value(this->sdfFrameId); + } + + if (this->sdfChildFrameId.empty()) + { + if (!this->bodyLinkName.empty()) + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->model.Name(_ecm) + "/" + this->bodyLinkName); + } + } + else + { + auto childFrame = msg.mutable_header()->add_data(); + childFrame->set_key("child_frame_id"); + childFrame->add_value(this->sdfChildFrameId); + } + + // Construct the Pose_V/tf message and publish it. + msgs::Pose_V tfMsg; + auto *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 messages + this->odomPub.Publish(msg); + this->tfPub.Publish(tfMsg); +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::UpdateVelocity( + const ignition::gazebo::UpdateInfo &_info, + const ignition::gazebo::EntityComponentManager &_ecm) +{ + IGN_PROFILE("TrackedVehicle::UpdateVelocity"); + + // Read values protected by the mutex + double linVel; + double angVel; + double steeringEfficiencyCopy; + bool hadNewCommand; + { + std::lock_guard lock(this->mutex); + linVel = this->targetVel.linear().x(); + angVel = this->targetVel.angular().z(); + steeringEfficiencyCopy = this->steeringEfficiency; + hadNewCommand = this->hasNewCommand; + this->hasNewCommand = false; + } + + const auto dt = std::chrono::duration(_info.dt).count(); + + // Limit the target velocity if needed. + this->limiterLin->Limit( + linVel, this->last0Cmd.lin, this->last1Cmd.lin, _info.dt); + this->limiterAng->Limit( + angVel, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); + + // decide whether commands to tracks should be sent + bool sendCommandsToTracks{hadNewCommand}; + if (!hadNewCommand) + { + // if the speed limiter has been limiting the speed (or acceleration), + // we let it saturate first and will stop publishing to tracks after that + if (std::abs(linVel - this->last0Cmd.lin) > 1e-6) + { + sendCommandsToTracks = true; + } + else if (std::abs(angVel - this->last0Cmd.ang) > 1e-6) + { + sendCommandsToTracks = true; + } + } + + // Update history of commands. + this->last1Cmd = last0Cmd; + this->last0Cmd.lin = linVel; + this->last0Cmd.ang = angVel; + + // only update and publish the following values when tracks should be + // commanded with updated commands; none of these values changes when + // linVel and angVel stay the same + if (sendCommandsToTracks) + { + // Convert the target velocities to track velocities. + this->rightSpeed = (linVel + angVel * this->tracksSeparation / + (2.0 * steeringEfficiencyCopy)); + this->leftSpeed = (linVel - angVel * this->tracksSeparation / + (2.0 * steeringEfficiencyCopy)); + + // radius of the turn the robot is doing + this->desiredRotationRadiusSigned = + (fabs(angVel) < 0.1) ? + // is driving straight + math::INF_D : + ( + (fabs(linVel) < 0.1) ? + // is rotating about a single point + 0 : + // general movement + linVel / angVel); + + const auto bodyPose = worldPose(this->bodyLink, _ecm); + const auto bodyYAxisGlobal = + bodyPose.Rot().RotateVector(ignition::math::Vector3d(0, 1, 0)); + // centerOfRotation may be +inf + this->centerOfRotation = + (bodyYAxisGlobal * desiredRotationRadiusSigned) + bodyPose.Pos(); + + for (const auto& track : this->leftTrackNames) + { + msgs::Double vel; + vel.set_data(this->leftSpeed); + this->velPublishers[track].Publish(vel); + + this->corPublishers[track].Publish( + msgs::Convert(this->centerOfRotation)); + } + + for (const auto& track : this->rightTrackNames) + { + msgs::Double vel; + vel.set_data(this->rightSpeed); + this->velPublishers[track].Publish(vel); + + this->corPublishers[track].Publish( + msgs::Convert(this->centerOfRotation)); + } + } + + // Odometry is computed as if the vehicle were a diff-drive vehicle with + // wheels as high as the tracks are. + this->odomLeftWheelPos += this->leftSpeed / (this->trackHeight / 2) * dt; + this->odomRightWheelPos += this->rightSpeed / (this->trackHeight / 2) * dt; + + if (this->debug) + { + igndbg << "Tracked Vehicle " << this->model.Name(_ecm) << ":" << std::endl; + igndbg << "- cmd vel v=" << linVel << ", w=" << angVel + << (hadNewCommand ? " (new command)" : "") << std::endl; + igndbg << "- left v=" << this->leftSpeed + << ", right v=" << this->rightSpeed + << (sendCommandsToTracks ? " (sent to tracks)" : "") << std::endl; + + ignition::msgs::Set(this->debugMarker.mutable_pose(), math::Pose3d( + this->centerOfRotation.X(), + this->centerOfRotation.Y(), + this->centerOfRotation.Z(), + 0, 0, 0)); + this->node.Request("/marker", this->debugMarker); + } +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::OnCmdVel(const msgs::Twist &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetVel = _msg; + this->hasNewCommand = true; +} + +////////////////////////////////////////////////// +void TrackedVehiclePrivate::OnSteeringEfficiency( + const ignition::msgs::Double& _msg) +{ + std::lock_guard lock(this->mutex); + this->steeringEfficiency = _msg.data(); + this->hasNewCommand = true; +} + +IGNITION_ADD_PLUGIN(TrackedVehicle, + ignition::gazebo::System, + TrackedVehicle::ISystemConfigure, + TrackedVehicle::ISystemPreUpdate, + TrackedVehicle::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(TrackedVehicle, + "ignition::gazebo::systems::TrackedVehicle") diff --git a/src/systems/tracked_vehicle/TrackedVehicle.hh b/src/systems/tracked_vehicle/TrackedVehicle.hh new file mode 100644 index 00000000000..878c5defc9d --- /dev/null +++ b/src/systems/tracked_vehicle/TrackedVehicle.hh @@ -0,0 +1,168 @@ +/* + * 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_TRACKEDVEHICLE_HH_ +#define IGNITION_GAZEBO_SYSTEMS_TRACKEDVEHICLE_HH_ + +#include + +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { +namespace systems +{ + // Forward declaration + class TrackedVehiclePrivate; + + /// \brief Tracked vehicle controller which can be attached to a model + /// with any number of left and right tracks. The system should be attached + /// to a model. Each track has to have a TrackController system configured and + /// running. + /// + /// So far, this system only supports tracks that are parallel along a common + /// axis (other designs are possible, but not implemented). + /// + /// # Examples + /// + /// See example usage in world example/tracked_vehicle_simple.sdf . + /// + /// # System Parameters + /// + /// ``: Configuration of a left track link. This element can + /// appear multiple times, and must appear at least once. + /// + /// ``: Configuration of a right track link. This element can + /// appear multiple times, and must appear at least once. + /// + /// ``, `` subelements: + /// - ``: The link representing the track. Required parameter. + /// - ``: The topic on which the track accepts velocity + /// commands (defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_vel`). + /// - ``: The topic on which the track accepts + /// center of rotation commands (defaults to + /// `/model/${model_name}/link/${link_name}/track_cmd_center_of_rotation`) + /// + /// ``: Distance between tracks, in meters. Required + /// parameter. + /// + /// ``: Height of the tracks, in meters (used for computing + /// odometry). Required parameter. + /// + /// ``: Initial steering efficiency. Defaults to 0.5. + /// + /// `` If 1, the system will output debugging info and visualizations. + /// Defaults to 0. + /// + /// ``: Limiter of linear velocity of the vehicle. Please + /// note that the tracks can each have their own speed limitations. If the + /// element is not specified, the velocities etc. have no implicit limits. + /// - ``/`` Min/max velocity of the vehicle (m/s). + /// If not specified, the velocity is not limited (however the physics + /// will, in the end, have some implicit limit). + /// - ``/`` Min/max acceleration of the + /// vehicle (m/s^2). If not specified, the acceleration is not limited + /// (however the physics will, in the end, have some implicit limit). + /// - ``/`` Min/max jerk of the vehicle (m/s^3). If not + /// specified, the acceleration is not limited (however the physics will, + /// in the end, have some implicit limit). + /// + /// ``: Limiter of angular velocity of the vehicle. Please + /// note that the tracks can each have their own speed limitations. If the + /// element is not specified, the velocities etc. have no implicit limits. + /// - ``/`` Min/max velocity of the vehicle + /// (rad/s). If not specified, the velocity is not limited (however the + /// physics will, in the end, have some implicit limit). + /// - ``/`` Min/max acceleration of the + /// vehicle (rad/s^2). If not specified, the velocity is not limited + /// (however the physics will, in the end, have some implicit limit). + /// - ``/`` Min/max jerk of the vehicle (rad/s^3). If not + /// specified, the velocity is not limited (however the physics + /// will, in the end, have some implicit limit). + /// + /// ``: Odometry publication frequency. This + /// element is optional, and the default value is 50Hz. + /// + /// ``: Custom topic that this system will subscribe to in order to + /// receive command velocity messages. This element is optional, and the + /// default value is `/model/{model_name}/cmd_vel`. + /// + /// ``: Custom topic that this system will + /// subscribe to in order to receive steering efficiency messages. + /// This element is optional, and the default value is + /// `/model/{model_name}/steering_efficiency`. + /// + /// ``: Custom topic on which this system will publish odometry + /// messages. This element is optional, and the default value is + /// `/model/{model_name}/odometry`. + /// + /// ``: Custom topic on which this system will publish the + /// transform from `frame_id` to `child_frame_id`. This element is optional, + /// and the default value is `/model/{model_name}/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 `{model_name}/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 `{model_name}/{link_name}`. + class TrackedVehicle + : public System, + public ISystemConfigure, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: TrackedVehicle(); + + /// \brief Destructor + public: ~TrackedVehicle() override; + + // Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + + // Documentation inherited + public: void PreUpdate( + const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + /// \brief Private data pointer + private: std::unique_ptr dataPtr; + }; + } +} +} +} + +#endif diff --git a/src/systems/user_commands/UserCommands.cc b/src/systems/user_commands/UserCommands.cc index 7516c9b4d88..c619aa33a4a 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -23,11 +23,13 @@ #include #include #include +#include #include #include #include +#include #include #include @@ -49,13 +51,16 @@ #include "ignition/gazebo/components/Pose.hh" #include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/components/PhysicsCmd.hh" +#include "ignition/gazebo/components/SphericalCoordinates.hh" #include "ignition/gazebo/components/World.hh" #include "ignition/gazebo/Conversions.hh" #include "ignition/gazebo/EntityComponentManager.hh" #include "ignition/gazebo/SdfEntityCreator.hh" +#include "ignition/gazebo/World.hh" #include "ignition/gazebo/components/ContactSensorData.hh" #include "ignition/gazebo/components/ContactSensor.hh" #include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/VisualCmd.hh" using namespace ignition; using namespace gazebo; @@ -68,6 +73,74 @@ namespace gazebo inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace systems { + +/// \brief Helper function to get an entity from an entity message. +/// +/// \TODO(anyone) Move to Util.hh and generalize for all entities, +/// not only top level +/// +/// The message is used as follows: +/// +/// if id not null +/// use id +/// else if name not null and type not null +/// use name + type +/// else +/// error +/// end +/// \param[in] _ecm Entity component manager +/// \param[in] _msg Entity message +/// \return Entity ID, or kNullEntity if a matching entity couldn't be +/// found. +Entity topLevelEntityFromMessage(const EntityComponentManager &_ecm, + const msgs::Entity &_msg) +{ + if (_msg.id() != kNullEntity) + { + return _msg.id(); + } + + if (!_msg.name().empty() && _msg.type() != msgs::Entity::NONE) + { + Entity entity{kNullEntity}; + if (_msg.type() == msgs::Entity::MODEL) + { + entity = _ecm.EntityByComponents(components::Model(), + components::Name(_msg.name())); + } + else if (_msg.type() == msgs::Entity::LIGHT) + { + entity = _ecm.EntityByComponents( + components::Name(_msg.name())); + + auto lightComp = _ecm.Component(entity); + if (nullptr == lightComp) + entity = kNullEntity; + } + else + { + ignerr << "Failed to handle entity type [" << _msg.type() << "]" + << std::endl; + } + return entity; + } + + ignerr << "Message missing either entity's ID or name + type" << std::endl; + return kNullEntity; +} + +/// \brief Pose3d equality comparison function. +/// \param[in] _a A pose to compare +/// \param[in] _b Another pose to compare +bool pose3Eql(const math::Pose3d &_a, const math::Pose3d &_b) +{ + return _a.Pos().Equal(_b.Pos(), 1e-6) && + math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && + math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && + math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && + math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); +} + /// \brief This class is passed to every command and contains interfaces that /// can be shared among all commands. For example, all create and remove /// commands can use the `creator` object. @@ -220,17 +293,6 @@ class PoseCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; - - /// \brief Pose3d equality comparison function. - public: std::function - pose3Eql { [](const math::Pose3d &_a, const math::Pose3d &_b) - { - return _a.Pos().Equal(_b.Pos(), 1e-6) && - math::equal(_a.Rot().X(), _b.Rot().X(), 1e-6) && - math::equal(_a.Rot().Y(), _b.Rot().Y(), 1e-6) && - math::equal(_a.Rot().Z(), _b.Rot().Z(), 1e-6) && - math::equal(_a.Rot().W(), _b.Rot().W(), 1e-6); - }}; }; /// \brief Command to modify the physics parameters of a simulation. @@ -246,6 +308,19 @@ class PhysicsCommand : public UserCommandBase public: bool Execute() final; }; +/// \brief Command to modify the spherical coordinates of a simulation. +class SphericalCoordinatesCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the new coordinates. + /// \param[in] _iface Pointer to user commands interface. + public: SphericalCoordinatesCommand(msgs::SphericalCoordinates *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; +}; + /// \brief Command to enable a collision component. class EnableCollisionCommand : public UserCommandBase { @@ -271,6 +346,64 @@ class DisableCollisionCommand : public UserCommandBase // Documentation inherited public: bool Execute() final; }; + + +/// \brief Command to modify a visual entity from simulation. +class VisualCommand : public UserCommandBase +{ + /// \brief Constructor + /// \param[in] _msg Message containing the visual parameters. + /// \param[in] _iface Pointer to user commands interface. + public: VisualCommand(msgs::Visual *_msg, + std::shared_ptr &_iface); + + // Documentation inherited + public: bool Execute() final; + + /// \brief Visual equality comparision function + /// TODO(anyone) Currently only checks for material colors equality, + /// need to extend to others + public: std::function + visualEql { [](const msgs::Visual &_a, const msgs::Visual &_b) + { + auto aMaterial = _a.material(), bMaterial = _b.material(); + return + _a.name() == _b.name() && + _a.id() == _b.id() && + math::equal( + aMaterial.ambient().r(), bMaterial.ambient().r(), 1e-6f) && + math::equal( + aMaterial.ambient().g(), bMaterial.ambient().g(), 1e-6f) && + math::equal( + aMaterial.ambient().b(), bMaterial.ambient().b(), 1e-6f) && + math::equal( + aMaterial.ambient().a(), bMaterial.ambient().a(), 1e-6f) && + math::equal( + aMaterial.diffuse().r(), bMaterial.diffuse().r(), 1e-6f) && + math::equal( + aMaterial.diffuse().g(), bMaterial.diffuse().g(), 1e-6f) && + math::equal( + aMaterial.diffuse().b(), bMaterial.diffuse().b(), 1e-6f) && + math::equal( + aMaterial.diffuse().a(), bMaterial.diffuse().a(), 1e-6f) && + math::equal( + aMaterial.specular().r(), bMaterial.specular().r(), 1e-6f) && + math::equal( + aMaterial.specular().g(), bMaterial.specular().g(), 1e-6f) && + math::equal( + aMaterial.specular().b(), bMaterial.specular().b(), 1e-6f) && + math::equal( + aMaterial.specular().a(), bMaterial.specular().a(), 1e-6f) && + math::equal( + aMaterial.emissive().r(), bMaterial.emissive().r(), 1e-6f) && + math::equal( + aMaterial.emissive().g(), bMaterial.emissive().g(), 1e-6f) && + math::equal( + aMaterial.emissive().b(), bMaterial.emissive().b(), 1e-6f) && + math::equal( + aMaterial.emissive().a(), bMaterial.emissive().a(), 1e-6f); + }}; +}; } } } @@ -281,7 +414,7 @@ class ignition::gazebo::systems::UserCommandsPrivate { /// \brief Callback for create service /// \param[in] _req Request containing entity description. - /// \param[in] _res True if message successfully received and queued. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the entity will be successfully spawned. /// \return True if successful. public: bool CreateService(const msgs::EntityFactory &_req, @@ -289,7 +422,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \brief Callback for multiple create service /// \param[in] _req Request containing one or more entity descriptions. - /// \param[in] _res True if message successfully received and queued. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the entities will be successfully spawned. /// \return True if successful. public: bool CreateServiceMultiple( @@ -297,7 +430,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \brief Callback for remove service /// \param[in] _req Request containing identification of entity to be removed. - /// \param[in] _res True if message successfully received and queued. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the entity will be successfully removed. /// \return True if successful. public: bool RemoveService(const msgs::Entity &_req, @@ -305,7 +438,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \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. + /// \param[out] _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); @@ -314,21 +447,29 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \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. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the entity will be successfully moved. /// \return True if successful. public: bool 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. + /// \param[out] _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 Callback for spherical coordinates service + /// \param[in] _req Request containing updates to the spherical coordinates. + /// \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 SphericalCoordinatesService( + const msgs::SphericalCoordinates &_req, msgs::Boolean &_res); + /// \brief Callback for enable collision service /// \param[in] _req Request containing collision entity. - /// \param[in] _res True if message successfully received and queued. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the collision will be successfully enabled. /// \return True if successful. public: bool EnableCollisionService( @@ -336,12 +477,19 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \brief Callback for disable collision service /// \param[in] _req Request containing collision entity. - /// \param[in] _res True if message successfully received and queued. + /// \param[out] _res True if message successfully received and queued. /// It does not mean that the collision will be successfully disabled. /// \return True if successful. public: bool DisableCollisionService( const msgs::Entity &_req, msgs::Boolean &_res); + /// \brief Callback for visual service + /// \param[in] _req Request containing visual updates of an entity + /// \param[out] _res True if message sucessfully received and queued. + /// It does not mean that the viusal will be successfully updated + /// \return True if successful. + public: bool VisualService(const msgs::Visual &_req, msgs::Boolean &_res); + /// \brief Queue of commands pending execution. public: std::vector> pendingCmds; @@ -467,6 +615,15 @@ void UserCommands::Configure(const Entity &_entity, ignmsg << "Physics service on [" << physicsService << "]" << std::endl; + // Spherical coordinates service + std::string sphericalCoordinatesService{"/world/" + validWorldName + + "/set_spherical_coordinates"}; + this->dataPtr->node.Advertise(sphericalCoordinatesService, + &UserCommandsPrivate::SphericalCoordinatesService, this->dataPtr.get()); + + ignmsg << "SphericalCoordinates service on [" << sphericalCoordinatesService + << "]" << std::endl; + // Enable collision service std::string enableCollisionService{ "/world/" + validWorldName + "/enable_collision"}; @@ -484,6 +641,14 @@ void UserCommands::Configure(const Entity &_entity, ignmsg << "Disable collision service on [" << disableCollisionService << "]" << std::endl; + + // Visual service + std::string visualService + {"/world/" + worldName + "/visual_config"}; + this->dataPtr->node.Advertise(visualService, + &UserCommandsPrivate::VisualService, this->dataPtr.get()); + + ignmsg << "Material service on [" << visualService << "]" << std::endl; } ////////////////////////////////////////////////// @@ -685,6 +850,42 @@ bool UserCommandsPrivate::PhysicsService(const msgs::Physics &_req, return true; } +////////////////////////////////////////////////// +bool UserCommandsPrivate::VisualService(const msgs::Visual &_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::SphericalCoordinatesService( + const msgs::SphericalCoordinates &_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) @@ -746,9 +947,41 @@ bool CreateCommand::Execute() } case msgs::EntityFactory::kCloneName: { - // TODO(louise) Implement clone - ignerr << "Cloning an entity is not yet supported." << std::endl; - return false; + auto validClone = false; + auto clonedEntity = kNullEntity; + auto entityToClone = this->iface->ecm->EntityByComponents( + components::Name(createMsg->clone_name())); + if (kNullEntity != entityToClone) + { + auto parentComp = + this->iface->ecm->Component(entityToClone); + + // TODO(anyone) add better support for creating non-top level entities. + // For now, we will only clone top level entities + if (parentComp && parentComp->Data() == this->iface->worldEntity) + { + auto parentEntity = parentComp->Data(); + clonedEntity = this->iface->ecm->Clone(entityToClone, + parentEntity, createMsg->name(), createMsg->allow_renaming()); + validClone = kNullEntity != clonedEntity; + } + } + + if (!validClone) + { + ignerr << "Request to clone an entity named [" + << createMsg->clone_name() << "] failed." << std::endl; + return false; + } + + if (createMsg->has_pose()) + { + // TODO(anyone) handle if relative_to is filled + auto pose = gazebo::convert(createMsg->pose()); + this->iface->ecm->SetComponentData(clonedEntity, + pose); + } + return true; } default: { @@ -882,6 +1115,35 @@ bool CreateCommand::Execute() auto poseComp = this->iface->ecm->Component(entity); *poseComp = components::Pose(msgs::Convert(createMsg->pose())); } + // Spherical coordinates + else if (createMsg->has_spherical_coordinates()) + { + auto scComp = this->iface->ecm->Component( + this->iface->worldEntity); + if (nullptr == scComp) + { + ignwarn << "Trying to create entity [" << desiredName + << "] with spherical coordinates, but world's spherical " + << "coordinates aren't set. Entity will be created at the world " + << "origin." << std::endl; + } + else + { + // deg to rad + math::Vector3d latLonEle{ + IGN_DTOR(createMsg->spherical_coordinates().latitude_deg()), + IGN_DTOR(createMsg->spherical_coordinates().longitude_deg()), + createMsg->spherical_coordinates().elevation()}; + + auto pos = scComp->Data().PositionTransform(latLonEle, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + + auto poseComp = this->iface->ecm->Component(entity); + *poseComp = components::Pose({pos.X(), pos.Y(), pos.Z(), 0, 0, + IGN_DTOR(createMsg->spherical_coordinates().heading_deg())}); + } + } igndbg << "Created entity [" << entity << "] named [" << desiredName << "]" << std::endl; @@ -906,42 +1168,7 @@ bool RemoveCommand::Execute() return false; } - Entity entity{kNullEntity}; - if (removeMsg->id() != kNullEntity) - { - entity = removeMsg->id(); - } - else if (!removeMsg->name().empty() && - removeMsg->type() != msgs::Entity::NONE) - { - if (removeMsg->type() == msgs::Entity::MODEL) - { - entity = this->iface->ecm->EntityByComponents(components::Model(), - components::Name(removeMsg->name())); - } - else if (removeMsg->type() == msgs::Entity::LIGHT) - { - entity = this->iface->ecm->EntityByComponents( - components::Name(removeMsg->name())); - - auto lightComp = this->iface->ecm->Component(entity); - if (nullptr == lightComp) - entity = kNullEntity; - } - else - { - ignerr << "Deleting entities of type [" << removeMsg->type() - << "] is not supported." << std::endl; - return false; - } - } - else - { - ignerr << "Remove command missing either entity's ID or name + type" - << std::endl; - return false; - } - + auto entity = topLevelEntityFromMessage(*this->iface->ecm, *removeMsg); if (entity == kNullEntity) { ignerr << "Entity named [" << removeMsg->name() << "] of type [" @@ -1107,7 +1334,7 @@ bool PoseCommand::Execute() else { /// \todo(anyone) Moving an object is not captured in a log file. - auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), this->pose3Eql) ? + auto state = poseCmdComp->SetData(msgs::Convert(*poseMsg), pose3Eql) ? ComponentState::OneTimeChange : ComponentState::NoChange; this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, @@ -1151,6 +1378,86 @@ bool PhysicsCommand::Execute() return true; } +////////////////////////////////////////////////// +SphericalCoordinatesCommand::SphericalCoordinatesCommand( + msgs::SphericalCoordinates *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool SphericalCoordinatesCommand::Execute() +{ + auto sphericalCoordinatesMsg = + dynamic_cast(this->msg); + if (nullptr == sphericalCoordinatesMsg) + { + ignerr << "Internal error, null SphericalCoordinates message" << std::endl; + return false; + } + + // World + if (!sphericalCoordinatesMsg->has_entity()) + { + World world(this->iface->worldEntity); + world.SetSphericalCoordinates(*this->iface->ecm, + msgs::Convert(*sphericalCoordinatesMsg)); + return true; + } + + // Entity + auto entity = topLevelEntityFromMessage(*this->iface->ecm, + sphericalCoordinatesMsg->entity()); + + if (!this->iface->ecm->HasEntity(entity)) + { + ignerr << "Unable to update the pose for entity [" << entity + << "]: entity doesn't exist." << std::endl; + return false; + } + + auto scComp = this->iface->ecm->Component( + this->iface->worldEntity); + if (nullptr == scComp) + { + ignerr << "Trying to move entity [" << entity + << "] using spherical coordinates, but world's spherical " + << "coordinates aren't set." << std::endl; + return false; + } + + // deg to rad + math::Vector3d latLonEle{ + IGN_DTOR(sphericalCoordinatesMsg->latitude_deg()), + IGN_DTOR(sphericalCoordinatesMsg->longitude_deg()), + sphericalCoordinatesMsg->elevation()}; + + auto pos = scComp->Data().PositionTransform(latLonEle, + math::SphericalCoordinates::SPHERICAL, + math::SphericalCoordinates::LOCAL2); + + math::Pose3d pose{pos.X(), pos.Y(), pos.Z(), 0, 0, + IGN_DTOR(sphericalCoordinatesMsg->heading_deg())}; + + auto poseCmdComp = + this->iface->ecm->Component(entity); + if (!poseCmdComp) + { + this->iface->ecm->CreateComponent(entity, components::WorldPoseCmd(pose)); + } + else + { + auto state = poseCmdComp->SetData(pose, pose3Eql) ? + ComponentState::OneTimeChange : + ComponentState::NoChange; + this->iface->ecm->SetChanged(entity, components::WorldPoseCmd::typeId, + state); + } + + return true; +} + ////////////////////////////////////////////////// EnableCollisionCommand::EnableCollisionCommand(msgs::Entity *_msg, std::shared_ptr &_iface) @@ -1253,6 +1560,47 @@ bool DisableCollisionCommand::Execute() return true; } +////////////////////////////////////////////////// +VisualCommand::VisualCommand(msgs::Visual *_msg, + std::shared_ptr &_iface) + : UserCommandBase(_msg, _iface) +{ +} + +////////////////////////////////////////////////// +bool VisualCommand::Execute() +{ + auto visualMsg = dynamic_cast(this->msg); + if (nullptr == visualMsg) + { + ignerr << "Internal error, null visual message" << std::endl; + return false; + } + + if (visualMsg->id() == kNullEntity) + { + ignerr << "Failed to find visual entity" << std::endl; + return false; + } + + Entity visualEntity = visualMsg->id(); + auto visualCmdComp = + this->iface->ecm->Component(visualEntity); + if (!visualCmdComp) + { + this->iface->ecm->CreateComponent( + visualEntity, components::VisualCmd(*visualMsg)); + } + else + { + auto state = visualCmdComp->SetData(*visualMsg, this->visualEql) ? + ComponentState::OneTimeChange : ComponentState::NoChange; + this->iface->ecm->SetChanged( + visualEntity, components::VisualCmd::typeId, state); + } + return true; +} + IGNITION_ADD_PLUGIN(UserCommands, System, UserCommands::ISystemConfigure, UserCommands::ISystemPreUpdate diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 86cdb3578d5..a341f8e5948 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -20,111 +20,6 @@ execute_process(COMMAND cmake -E remove_directory ${CMAKE_BINARY_DIR}/test_resul execute_process(COMMAND cmake -E make_directory ${CMAKE_BINARY_DIR}/test_results) include_directories(${GTEST_INCLUDE_DIRS}) -################################# FindDRI support ############################# -# Check for existance of glxinfo application -# Check for existance of support for pyopengl -function(FindDRI) - -message(STATUS "Looking for display capabilities") - -if((DEFINED FORCE_GRAPHIC_TESTS_COMPILATION) - AND (${FORCE_GRAPHIC_TESTS_COMPILATION})) - set(VALID_DISPLAY TRUE PARENT_SCOPE) - set(VALID_DRI_DISPLAY TRUE PARENT_SCOPE) - message(STATUS " + Force requested. All capabilities on without checking") - return() -endif() - -set(VALID_DISPLAY FALSE) -set(VALID_DRI_DISPLAY FALSE) -set(CHECKER_ERROR "(no glxinfo or pyopengl)") - -if((DEFINED ENV{DISPLAY}) AND NOT ("$ENV{DISPLAY}" STREQUAL "")) - find_program(XWININFO xwininfo) - if (XWININFO) - execute_process( - COMMAND xwininfo -root - RESULT_VARIABLE DISPLAY_FAIL_RESULT - ERROR_QUIET OUTPUT_QUIET - ) - else() - message(STATUS "Could not find 'xwininfo', which is needed for FindDRI. Please install the package 'x11-utils'") - return() - endif() - - if(NOT DISPLAY_FAIL_RESULT) - message(STATUS " + found a display available ($DISPLAY is set)") - set(VALID_DISPLAY TRUE) - - # Continue check for DRI support in the display Try to run glxinfo. If not - # found, variable will be empty - find_program(GLXINFO glxinfo) - - # If not display found, it will throw an error - # Another grep pattern: "direct rendering:[[:space:]]*Yes[[:space:]]*" - if(GLXINFO) - execute_process( - COMMAND glxinfo - COMMAND grep "direct rendering:[[:space:]]*Yes[[:space:]]*" - ERROR_QUIET - OUTPUT_VARIABLE GLX - ) - - if(GLX) - message(STATUS " + found a valid dri display (glxinfo)") - set(VALID_DRI_DISPLAY TRUE) - else() - set(CHECKER_ERROR "using glxinfo") - endif() - else() - message(STATUS - "Could not find glxinfo. Trying with 'gl-test.py' which may not work." - "If 'gl-test.py' fails to find DRI, try installng the package " - "'mesa-utils' for glxinfo") - execute_process( - # RESULT_VARIABLE is store in a FAIL variable since the command - # returns 0 if ok and 1 if error (inverse than cmake IF) - COMMAND ${PROJECT_SOURCE_DIR}/tools/gl-test.py - RESULT_VARIABLE GL_FAIL_RESULT - ERROR_VARIABLE GL_ERROR - OUTPUT_QUIET - ) - - if(NOT GL_FAIL_RESULT) - message(STATUS " + found a valid dri display (pyopengl)") - set(VALID_DRI_DISPLAY TRUE) - elseif(${GL_ERROR}) - # Check error string: no python module means no pyopengl - string( - FIND ${GL_ERROR} "ImportError: No module named OpenGL.GLUT" ERROR_POS - ) - # -1 will imply pyopengl is present but real DRI test fails - if("${ERROR_POS}" STREQUAL "-1") - set(CHECKER_ERROR "using pyopengl") - endif() - endif() - endif() - endif() -endif() - -if(NOT VALID_DISPLAY) - message(STATUS " ! valid display not found") -endif() - -if(NOT VALID_DRI_DISPLAY) - message(STATUS " ! valid dri display not found ${CHECKER_ERROR}") -endif() - -# Set variables to parent scope -set(VALID_DISPLAY ${VALID_DISPLAY} PARENT_SCOPE) -set(VALID_DRI_DISPLAY ${VALID_DRI_DISPLAY} PARENT_SCOPE) - -endfunction() - -############################## End FindDRI support ############################# - -FindDRI() - add_subdirectory(benchmark) add_subdirectory(integration) add_subdirectory(performance) diff --git a/test/find_dri.cmake b/test/find_dri.cmake new file mode 100644 index 00000000000..ce43b372dbf --- /dev/null +++ b/test/find_dri.cmake @@ -0,0 +1,104 @@ +################################# FindDRI support ############################# +# Check for existance of glxinfo application +# Check for existance of support for pyopengl +function(FindDRI) + +message(STATUS "Looking for display capabilities") + +if((DEFINED FORCE_GRAPHIC_TESTS_COMPILATION) + AND (${FORCE_GRAPHIC_TESTS_COMPILATION})) + set(VALID_DISPLAY TRUE PARENT_SCOPE) + set(VALID_DRI_DISPLAY TRUE PARENT_SCOPE) + message(STATUS " + Force requested. All capabilities on without checking") + return() +endif() + +set(VALID_DISPLAY FALSE) +set(VALID_DRI_DISPLAY FALSE) +set(CHECKER_ERROR "(no glxinfo or pyopengl)") + +if((DEFINED ENV{DISPLAY}) AND NOT ("$ENV{DISPLAY}" STREQUAL "")) + find_program(XWININFO xwininfo) + if (XWININFO) + execute_process( + COMMAND xwininfo -root + RESULT_VARIABLE DISPLAY_FAIL_RESULT + ERROR_QUIET OUTPUT_QUIET + ) + else() + message(STATUS "Could not find 'xwininfo', which is needed for FindDRI. Please install the package 'x11-utils'") + return() + endif() + + if(NOT DISPLAY_FAIL_RESULT) + message(STATUS " + found a display available ($DISPLAY is set)") + set(VALID_DISPLAY TRUE) + + # Continue check for DRI support in the display Try to run glxinfo. If not + # found, variable will be empty + find_program(GLXINFO glxinfo) + + # If not display found, it will throw an error + # Another grep pattern: "direct rendering:[[:space:]]*Yes[[:space:]]*" + if(GLXINFO) + execute_process( + COMMAND glxinfo + COMMAND grep "direct rendering:[[:space:]]*Yes[[:space:]]*" + ERROR_QUIET + OUTPUT_VARIABLE GLX + ) + + if(GLX) + message(STATUS " + found a valid dri display (glxinfo)") + set(VALID_DRI_DISPLAY TRUE) + else() + set(CHECKER_ERROR "using glxinfo") + endif() + else() + message(STATUS + "Could not find glxinfo. Trying with 'gl-test.py' which may not work." + "If 'gl-test.py' fails to find DRI, try installng the package " + "'mesa-utils' for glxinfo") + execute_process( + # RESULT_VARIABLE is store in a FAIL variable since the command + # returns 0 if ok and 1 if error (inverse than cmake IF) + COMMAND ${PROJECT_SOURCE_DIR}/tools/gl-test.py + RESULT_VARIABLE GL_FAIL_RESULT + ERROR_VARIABLE GL_ERROR + OUTPUT_QUIET + ) + + if(NOT GL_FAIL_RESULT) + message(STATUS " + found a valid dri display (pyopengl)") + set(VALID_DRI_DISPLAY TRUE) + elseif(${GL_ERROR}) + # Check error string: no python module means no pyopengl + string( + FIND ${GL_ERROR} "ImportError: No module named OpenGL.GLUT" ERROR_POS + ) + # -1 will imply pyopengl is present but real DRI test fails + if("${ERROR_POS}" STREQUAL "-1") + set(CHECKER_ERROR "using pyopengl") + endif() + endif() + endif() + endif() +endif() + +if(NOT VALID_DISPLAY) + message(STATUS " ! valid display not found") +endif() + +if(NOT VALID_DRI_DISPLAY) + message(STATUS " ! valid dri display not found ${CHECKER_ERROR}") +endif() + +# Set variables to parent scope +set(VALID_DISPLAY ${VALID_DISPLAY} PARENT_SCOPE) +set(VALID_DRI_DISPLAY ${VALID_DRI_DISPLAY} PARENT_SCOPE) + +endfunction() + +############################## End FindDRI support ############################# + + diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 9753c3ef2c9..acae1df569d 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -8,6 +8,7 @@ set(tests battery_plugin.cc breadcrumbs.cc buoyancy.cc + buoyancy_engine.cc collada_world_exporter.cc components.cc contact_system.cc @@ -18,6 +19,7 @@ set(tests events.cc examples_build.cc follow_actor_system.cc + force_torque_system.cc fuel_cached_server.cc halt_motion.cc imu_system.cc @@ -49,7 +51,10 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc + spherical_coordinates.cc + thruster.cc touch_plugin.cc + tracked_vehicle_system.cc triggered_publisher.cc user_commands.cc velocity_control_system.cc @@ -61,6 +66,7 @@ set(tests # Tests that require a valid display set(tests_needing_display + camera_sensor_background.cc camera_video_record_system.cc depth_camera.cc gpu_lidar.cc @@ -102,3 +108,25 @@ ign_build_tests(TYPE INTEGRATION LIB_DEPS ${EXTRA_TEST_LIB_DEPS} ) + + +# For INTEGRATION_physics_system, we need to check what version of DART is +# available so that we can disable tests that are unsupported by the particular +# version of physics engine +ign_find_package(DART QUIET) +if (DART_FOUND) + # Only adding include directories, no need to link against DART to check version + target_include_directories(INTEGRATION_physics_system SYSTEM PRIVATE ${DART_INCLUDE_DIRS}) + target_compile_definitions(INTEGRATION_physics_system PRIVATE HAVE_DART) +endif() + +target_link_libraries(INTEGRATION_tracked_vehicle_system + ignition-physics${IGN_PHYSICS_VER}::core + ignition-plugin${IGN_PLUGIN_VER}::loader +) +# The default timeout (240s) doesn't seem to be enough for this test. +set_tests_properties(INTEGRATION_tracked_vehicle_system PROPERTIES TIMEOUT 300) + +if(TARGET INTEGRATION_examples_build) + set_tests_properties(INTEGRATION_examples_build PROPERTIES TIMEOUT 320) +endif() diff --git a/test/integration/buoyancy.cc b/test/integration/buoyancy.cc index 77445fdd61b..5ab6ba4ca27 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -42,7 +42,7 @@ class BuoyancyTest : public InternalFixture<::testing::Test> }; ///////////////////////////////////////////////// -TEST_F(BuoyancyTest, Movement) +TEST_F(BuoyancyTest, UniformWorldMovement) { // Start server ServerConfig serverConfig; @@ -57,7 +57,7 @@ TEST_F(BuoyancyTest, Movement) using namespace std::chrono_literals; server.SetUpdatePeriod(1ns); - std::size_t iterations = 1000; + std::size_t iterations = 1001; bool finished = false; test::Relay testSystem; @@ -183,7 +183,7 @@ TEST_F(BuoyancyTest, Movement) if (_info.iterations == iterations) { - EXPECT_NEAR(-1.63, submarineSinkingPose->Data().Pos().Z(), 1e-2); + EXPECT_NEAR(-1.64, submarineSinkingPose->Data().Pos().Z(), 1e-2); EXPECT_NEAR(4.90, submarineBuoyantPose->Data().Pos().Z(), 1e-2); EXPECT_NEAR(171.4, duckPose->Data().Pos().Z(), 1e-2); finished = true; @@ -194,3 +194,87 @@ TEST_F(BuoyancyTest, Movement) server.Run(true, iterations, false); EXPECT_TRUE(finished); } + +///////////////////////////////////////////////// +TEST_F(BuoyancyTest, GradedBuoyancy) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "graded_buoyancy.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 1000; + + bool finished = false; + test::Relay testSystem; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity neutralBox = _ecm.EntityByComponents( + components::Model(), components::Name("neutral_buoyancy")); + + Entity bobbingBall = _ecm.EntityByComponents( + components::Model(), components::Name("lighter_than_water")); + + Entity heliumBalloon = _ecm.EntityByComponents( + components::Model(), components::Name("balloon_lighter_than_air")); + + Entity noBuoyancy = _ecm.EntityByComponents( + components::Model(), components::Name("box_no_buoyancy")); + + ASSERT_NE(neutralBox, kNullEntity); + ASSERT_NE(bobbingBall, kNullEntity); + ASSERT_NE(heliumBalloon, kNullEntity); + ASSERT_NE(noBuoyancy, kNullEntity); + + auto neutralBoxPose = _ecm.Component(neutralBox); + ASSERT_NE(neutralBoxPose, nullptr); + + auto bobbingBallPose = _ecm.Component(bobbingBall); + ASSERT_NE(bobbingBallPose , nullptr); + + auto heliumBalloonPose = _ecm.Component(heliumBalloon); + ASSERT_NE(heliumBalloonPose , nullptr); + + auto noBuoyancyPose = _ecm.Component(noBuoyancy); + ASSERT_NE(noBuoyancyPose , nullptr); + + // The "neutralBox" should stay in its starting location. + EXPECT_NEAR(0, neutralBoxPose->Data().Pos().X(), 1e-1); + EXPECT_NEAR(5, neutralBoxPose->Data().Pos().Y(), 1e-1); + EXPECT_NEAR(-3, neutralBoxPose->Data().Pos().Z(), 1e-1); + + if (_info.iterations > 10) + { + // Helium balloon should float up + EXPECT_GT(heliumBalloonPose->Data().Pos().Z(), + neutralBoxPose->Data().Pos().Z()); + + // Bobbing ball should stay within a certain band. + EXPECT_GT(bobbingBallPose->Data().Pos().Z(), -1.2); + EXPECT_LT(bobbingBallPose->Data().Pos().Z(), 0.5); + + // No buoyancy box should sink + EXPECT_LT(noBuoyancyPose->Data().Pos().Z(), + neutralBoxPose->Data().Pos().Z()); + } + + if (_info.iterations == iterations) + { + finished = true; + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, iterations, false); + EXPECT_TRUE(finished); +} diff --git a/test/integration/buoyancy_engine.cc b/test/integration/buoyancy_engine.cc new file mode 100644 index 00000000000..822666d22c7 --- /dev/null +++ b/test/integration/buoyancy_engine.cc @@ -0,0 +1,152 @@ +/* + * 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/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/components/Model.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class BuoyancyEngineTest : public InternalFixture<::testing::Test> +{ + // Documentation inherited + protected: void SetUp() override + { + InternalFixture::SetUp(); + this->pub = this->node.Advertise( + "/model/buoyant_box/buoyancy_engine/"); + } + + /// \brief Node for communication + public: ignition::transport::Node node; + + /// \brief Publishes commands + public: ignition::transport::Node::Publisher pub; +}; + +///////////////////////////////////////////////// +TEST_F(BuoyancyEngineTest, TestDownward) +{ + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "buoyancy_engine.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 10000; + + test::Relay testSystem; + std::vector poses; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity buoyantBox = _ecm.EntityByComponents( + components::Model(), components::Name("buoyant_box")); + EXPECT_NE(kNullEntity, buoyantBox); + + auto submarineBuoyantPose = _ecm.Component(buoyantBox); + EXPECT_NE(submarineBuoyantPose, nullptr); + if (submarineBuoyantPose == nullptr) + { + ignerr << "Unable to get pose" <Data()); + }); + + server.AddSystem(testSystem.systemPtr); + ignition::msgs::Double volume; + volume.set_data(0); + this->pub.Publish(volume); + server.Run(true, iterations, false); + + EXPECT_LT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z()); + EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3); + EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3); +} + +///////////////////////////////////////////////// +TEST_F(BuoyancyEngineTest, TestUpward) +{ + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/buoyancy_engine.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + using namespace std::chrono_literals; + server.SetUpdatePeriod(1ns); + + std::size_t iterations = 10000; + + test::Relay testSystem; + std::vector poses; + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &/*_info*/, + const gazebo::EntityComponentManager &_ecm) + { + // Check pose + Entity buoyantBox = _ecm.EntityByComponents( + components::Model(), components::Name("buoyant_box")); + EXPECT_NE(kNullEntity, buoyantBox); + + auto submarineBuoyantPose = _ecm.Component(buoyantBox); + EXPECT_NE(submarineBuoyantPose, nullptr); + if (submarineBuoyantPose == nullptr) + { + ignerr << "Unable to get pose" <Data()); + }); + + server.AddSystem(testSystem.systemPtr); + ignition::msgs::Double volume; + volume.set_data(10); + this->pub.Publish(volume); + server.Run(true, iterations, false); + + EXPECT_GT(poses.rbegin()->Pos().Z(), poses.begin()->Pos().Z()); + EXPECT_NEAR(poses.rbegin()->Pos().X(), poses.begin()->Pos().X(), 1e-3); + EXPECT_NEAR(poses.rbegin()->Pos().Y(), poses.begin()->Pos().Y(), 1e-3); +} + diff --git a/test/integration/camera_sensor_background.cc b/test/integration/camera_sensor_background.cc new file mode 100644 index 00000000000..1b7bbdc5e6c --- /dev/null +++ b/test/integration/camera_sensor_background.cc @@ -0,0 +1,104 @@ +/* + * 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 "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace std::chrono_literals; + +std::mutex mutex; +int cbCount = 0; + +////////////////////////////////////////////////// +class CameraSensorBackgroundFixture : + public InternalFixture> +{ +}; + +///////////////////////////////////////////////// +void cameraCb(const msgs::Image & _msg) +{ + ASSERT_EQ(msgs::PixelFormatType::RGB_INT8, + _msg.pixel_format_type()); + + for (unsigned int y = 0; y < _msg.height(); ++y) + { + for (unsigned int x = 0; x < _msg.width(); ++x) + { + // The "/test/worlds/camera_sensor_empty_scene.sdf" world has set a + // background color of 1,0,0,1. So, all the pixels returned by the + // camera should be red. + unsigned char r = _msg.data()[y * _msg.step() + x*3]; + EXPECT_EQ(255, static_cast(r)); + + unsigned char g = _msg.data()[y * _msg.step() + x*3+1]; + EXPECT_EQ(0, static_cast(g)); + + unsigned char b = _msg.data()[y * _msg.step() + x*3+2]; + EXPECT_EQ(0, static_cast(b)); + } + } + std::lock_guard lock(mutex); + cbCount++; +} + +///////////////////////////////////////////////// +// Test the ability to set the background color using the sensor system +// plugin. +TEST_F(CameraSensorBackgroundFixture, + IGN_UTILS_TEST_DISABLED_ON_MAC(RedBackground)) +{ + // Start server + gazebo::ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "camera_sensor_empty_scene.sdf"); + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // subscribe to the camera topic + transport::Node node; + cbCount = 0; + node.Subscribe("/camera", &cameraCb); + + // Run server and verify that we are receiving a message + // from the depth camera + server.Run(true, 100, false); + + int i = 0; + while (i < 100 && cbCount <= 0) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + i++; + } + + std::lock_guard lock(mutex); + EXPECT_GE(cbCount, 1); +} diff --git a/test/integration/components.cc b/test/integration/components.cc index a3b62444f85..93a507e83e7 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -18,6 +18,8 @@ #include #include +#include +#include #include @@ -30,6 +32,7 @@ #include #include #include +#include #include #include "ignition/gazebo/components/Actor.hh" @@ -47,6 +50,10 @@ #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" #include "ignition/gazebo/components/JointAxis.hh" +#include "ignition/gazebo/components/JointEffortLimitsCmd.hh" +#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" +#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh" #include "ignition/gazebo/components/JointType.hh" #include "ignition/gazebo/components/JointVelocity.hh" #include "ignition/gazebo/components/JointVelocityCmd.hh" @@ -591,6 +598,121 @@ TEST_F(ComponentsTest, JointAxis) EXPECT_EQ(comp3.Data().XyzExpressedIn(), "__model__"); } +///////////////////////////////////////////////// +TEST_F(ComponentsTest, JointEffortLimitsCmd) +{ + // Create components + auto comp1 = components::JointEffortLimitsCmd(); + auto comp2 = components::JointEffortLimitsCmd(); + + // Equality operators + EXPECT_EQ(comp1, comp2); + EXPECT_TRUE(comp1 == comp2); + EXPECT_FALSE(comp1 != comp2); + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("0", ostr.str()); + + comp2.Data().push_back(math::Vector2d(-1.0, 1.0)); + + std::ostringstream ostr2; + comp2.Serialize(ostr2); + EXPECT_EQ("1 -1 1", ostr2.str()); + + comp2.Data().push_back(math::Vector2d(-2.5, 2.5)); + + std::ostringstream ostr3; + comp2.Serialize(ostr3); + EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str()); + + std::istringstream istr("ignored"); + components::JointEffortLimitsCmd comp3; + comp3.Deserialize(istr); +} + +///////////////////////////////////////////////// +TEST_F(ComponentsTest, JointPositionLimitsCmd) +{ + // Create components + auto comp1 = components::JointPositionLimitsCmd(); + auto comp2 = components::JointPositionLimitsCmd(); + components::JointPositionLimitsCmd comp3; + + // Equality operators + EXPECT_EQ(comp1, comp2); + EXPECT_TRUE(comp1 == comp2); + EXPECT_FALSE(comp1 != comp2); + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("0", ostr.str()); + + auto istr = std::istringstream(ostr.str()); + comp3.Deserialize(istr); + EXPECT_EQ(comp1, comp3); + + comp2.Data().push_back(math::Vector2d(-1.0, 1.0)); + + std::ostringstream ostr2; + comp2.Serialize(ostr2); + EXPECT_EQ("1 -1 1", ostr2.str()); + + istr = std::istringstream(ostr2.str()); + comp3.Deserialize(istr); + EXPECT_EQ(comp2, comp3); + + comp2.Data().push_back(math::Vector2d(-2.5, 2.5)); + + std::ostringstream ostr3; + comp2.Serialize(ostr3); + EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str()); + + istr = std::istringstream(ostr3.str()); + comp3.Deserialize(istr); + EXPECT_EQ(comp2, comp3); + + istr = std::istringstream("ignored"); + comp3.Deserialize(istr); + EXPECT_EQ(comp1, comp3); +} + +///////////////////////////////////////////////// +TEST_F(ComponentsTest, JointVelocityLimitsCmd) +{ + // Create components + auto comp1 = components::JointVelocityLimitsCmd(); + auto comp2 = components::JointVelocityLimitsCmd(); + + // Equality operators + EXPECT_EQ(comp1, comp2); + EXPECT_TRUE(comp1 == comp2); + EXPECT_FALSE(comp1 != comp2); + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + EXPECT_EQ("0", ostr.str()); + + comp2.Data().push_back(math::Vector2d(-1.0, 1.0)); + + std::ostringstream ostr2; + comp2.Serialize(ostr2); + EXPECT_EQ("1 -1 1", ostr2.str()); + + comp2.Data().push_back(math::Vector2d(-2.5, 2.5)); + + std::ostringstream ostr3; + comp2.Serialize(ostr3); + EXPECT_EQ("2 -1 1 -2.5 2.5", ostr3.str()); + + std::istringstream istr("ignored"); + components::JointVelocityLimitsCmd comp3; + comp3.Deserialize(istr); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, JointType) { @@ -1097,6 +1219,66 @@ TEST_F(ComponentsTest, Model) comp3.Deserialize(istr); } +///////////////////////////////////////////////// +TEST_F(ComponentsTest, ModelSdf) +{ + std::ostringstream stream; + std::string version = SDF_VERSION; + stream + << "" + << "" + << " " + << " " + << " 0.001" + << " 1.0" + << " " + << " " + << " " + << " " + << " " + << " " + << " 0.1 0 0 0 0 0" + << " 0.2 0.3 0.4 1" + << " 0.3 0.4 0.5 1" + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + ASSERT_TRUE(sdf::readString(stream.str(), sdfParsed)); + + // model + EXPECT_TRUE(sdfParsed->Root()->HasElement("world")); + sdf::ElementPtr worldElem = sdfParsed->Root()->GetElement("world"); + EXPECT_TRUE(worldElem->HasElement("model")); + sdf::ElementPtr modelElem = worldElem->GetElement("model"); + EXPECT_TRUE(modelElem->HasAttribute("name")); + EXPECT_EQ(modelElem->Get("name"), "my_model"); + + sdf::Model model; + model.Load(modelElem); + EXPECT_EQ("my_model", model.Name()); + + // Create components + auto comp1 = components::ModelSdf(model); + components::ModelSdf comp2; + + // Stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + comp2.Deserialize(istr); + + EXPECT_EQ("my_model", comp2.Data().Name()); + EXPECT_EQ(1u, comp2.Data().LinkCount()); +} + ///////////////////////////////////////////////// TEST_F(ComponentsTest, Name) { @@ -1600,3 +1782,25 @@ TEST_F(ComponentsTest, ParticleEmitterCmd) EXPECT_EQ(comp1.Data().emitting().data(), comp3.Data().emitting().data()); EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); } + +////////////////////////////////////////////////// +TEST_F(ComponentsTest, JointTransmittedWrench) +{ + msgs::Wrench wrench; + msgs::Set(wrench.mutable_torque(), {10, 20, 30}); + msgs::Set(wrench.mutable_force(), {1, 2, 3}); + + // // Create components. + auto comp1 = components::JointTransmittedWrench(wrench); + + // Stream operators. + std::ostringstream ostr; + comp1.Serialize(ostr); + + std::istringstream istr(ostr.str()); + components::JointTransmittedWrench comp2; + comp2.Deserialize(istr); + EXPECT_EQ(msgs::Convert(comp2.Data().force()), msgs::Convert(wrench.force())); + EXPECT_EQ(msgs::Convert(comp2.Data().torque()), + msgs::Convert(wrench.torque())); +} diff --git a/test/integration/examples_build.cc b/test/integration/examples_build.cc index 7ff0269cdff..55e3ef99f31 100644 --- a/test/integration/examples_build.cc +++ b/test/integration/examples_build.cc @@ -147,10 +147,13 @@ void ExamplesBuild::Build(const std::string &_type) auto base = ignition::common::basename(*dirIter); math::SemanticVersion cmakeVersion{std::string(CMAKE_VERSION)}; - if (base == "gtest_setup" && cmakeVersion < math::SemanticVersion(3, 11, 0)) + if (cmakeVersion < math::SemanticVersion(3, 11, 0) && + (base == "custom_sensor_system" || + base == "gtest_setup")) { - igndbg << "Skipping [gtest_setup] test, which requires CMake version >= " - << "3.11.0. Currently using CMake " << cmakeVersion << std::endl; + igndbg << "Skipping [" << base << "] test, which requires CMake version " + << ">= 3.11.0. Currently using CMake " << cmakeVersion + << std::endl; continue; } diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc new file mode 100644 index 00000000000..a6f769e0ed3 --- /dev/null +++ b/test/integration/force_torque_system.cc @@ -0,0 +1,177 @@ +/* + * 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 "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Sensor.hh" +#include "ignition/gazebo/components/ForceTorque.hh" + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/test_config.hh" + +#include "helpers/Relay.hh" +#include "helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class ForceTorqueTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(ForceTorqueTest, MeasureWeight) +{ + using namespace std::chrono_literals; + // Start server + ServerConfig serverConfig; + const auto sdfFile = + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/force_torque.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + server.SetUpdatePeriod(1us); + + // Having iters exactly in sync with update rate can lead to a race condition + // in the test between simulation and transport + size_t iters = 999u; + size_t updates = 100u; + + std::vector wrenches; + wrenches.reserve(updates); + std::mutex wrenchMutex; + std::condition_variable cv; + auto wrenchCb = std::function( + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) + { + std::lock_guard lock(wrenchMutex); + wrenches.push_back(_msg); + if (wrenches.size() >= updates) + { + cv.notify_all(); + } + }); + + transport::Node node; + node.Subscribe("/force_torque1", wrenchCb); + + // Run server + server.Run(true, iters, false); + ASSERT_EQ(iters, *server.IterationCount()); + + { + std::unique_lock lock(wrenchMutex); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); + + const double kSensorMass = 0.2; + const double kWeightMass = 10; + const double kGravity = 9.8; + const auto &wrench = wrenches.back(); + const math::Vector3 expectedForce = + math::Vector3d{0, 0, kGravity * (kSensorMass + kWeightMass)}; + EXPECT_EQ(expectedForce, msgs::Convert(wrench.force())); + EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque())); + } +} + +///////////////////////////////////////////////// +TEST_F(ForceTorqueTest, SensorPoseOffset) +{ + using namespace std::chrono_literals; + // Start server + ServerConfig serverConfig; + const auto sdfFile = + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/force_torque.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + server.SetUpdatePeriod(1us); + + // Having iters exactly in sync with update rate can lead to a race condition + // in the test between simulation and transport + size_t iters = 999u; + size_t updates = 100u; + + std::vector wrenches; + wrenches.reserve(updates); + std::mutex wrenchMutex; + std::condition_variable cv; + auto wrenchCb = std::function( + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) + { + std::lock_guard lock(wrenchMutex); + wrenches.push_back(_msg); + if (wrenches.size() >= updates) + { + cv.notify_all(); + } + }); + + transport::Node node; + node.Subscribe("/force_torque2", wrenchCb); + + // Run server + server.Run(true, iters, false); + ASSERT_EQ(iters, *server.IterationCount()); + + const double kSensorMass = 0.2; + const double kWeightMass = 10; + const double kGravity = 9.8; + { + std::unique_lock lock(wrenchMutex); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); + + const double kMomentArm = 0.1; + const auto &wrench = wrenches.back(); + const math::Vector3 expectedForce = + math::Vector3d{0, 0, kGravity * (kSensorMass + kWeightMass)}; + EXPECT_EQ(expectedForce, msgs::Convert(wrench.force())); + EXPECT_NEAR(kMomentArm * expectedForce.Z(), wrench.torque().y(), 1e-3); + wrenches.clear(); + } + + node.Unsubscribe("/force_torque2"); + node.Subscribe("/force_torque3", wrenchCb); + + server.Run(true, iters, false); + ASSERT_EQ(2 * iters, *server.IterationCount()); + { + std::unique_lock lock(wrenchMutex); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); + + const auto &wrench = wrenches.back(); + + const math::Vector3 expectedForce = + math::Vector3d{0, 0, kGravity * (kSensorMass + kWeightMass)}; + EXPECT_EQ(expectedForce, msgs::Convert(wrench.force())); + EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque())); + wrenches.clear(); + } +} diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 23c52a16477..3cdd522c8ee 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -206,3 +206,41 @@ TEST_F(ImuTest, ModelFalling) EXPECT_EQ(imuMsgs.back().entity_name(), scopedName); mutex.unlock(); } + +///////////////////////////////////////////////// +// The test checks to make sure orientation is not published if it is deabled +TEST_F(ImuTest, OrientationDisabled) +{ + imuMsgs.clear(); + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_no_orientation.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + auto topic = + "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu"; + + // subscribe to imu topic + transport::Node node; + node.Subscribe(topic, &imuCb); + + // step world and verify imu's orientation is not published + // Run server + size_t iters200 = 200u; + server.Run(true, iters200, false); + + // Check we received messages + EXPECT_GT(imuMsgs.size(), 0u); + mutex.lock(); + for (const auto &msg : imuMsgs) + { + EXPECT_FALSE(msg.has_orientation()); + } + mutex.unlock(); +} diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 679293f582a..5b93173f4a4 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -47,7 +47,7 @@ class JointPositionControllerTestFixture ///////////////////////////////////////////////// // Tests that the JointPositionController accepts joint position commands -TEST_F(JointPositionControllerTestFixture, JointPositionCommand) +TEST_F(JointPositionControllerTestFixture, JointPositionForceCommand) { using namespace std::chrono_literals; @@ -120,3 +120,79 @@ TEST_F(JointPositionControllerTestFixture, JointPositionCommand) EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } + +///////////////////////////////////////////////// +// Tests that the JointPositionController accepts joint position commands +TEST_F(JointPositionControllerTestFixture, JointPositonVelocityCommand) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_position_controller_velocity.sdf"; + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j1"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + EXPECT_EQ(_name->Data(), jointName); + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/joint_position_controller_test/joint/j1/0/cmd_pos"); + + const double targetPosition{2.0}; + msgs::Double msg; + msg.set_data(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 1000; + server.Run(true, testIters , false); + + EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); +} diff --git a/test/integration/log_system.cc b/test/integration/log_system.cc index 54918929700..2d920f043bb 100644 --- a/test/integration/log_system.cc +++ b/test/integration/log_system.cc @@ -264,6 +264,7 @@ class LogSystemTest : public InternalFixture<::testing::Test> ///////////////////////////////////////////////// TEST_F(LogSystemTest, LogPlaybackStatistics) { + // TODO(anyone) see LogSystemTest.LogControl comment about re-recording auto logPath = common::joinPaths(PROJECT_SOURCE_PATH, "test", "media", "rolling_shapes_log"); @@ -307,8 +308,8 @@ TEST_F(LogSystemTest, LogPlaybackStatistics) EXPECT_EQ(0, startTimePair.first); EXPECT_EQ(0, startTimePair.second); - EXPECT_EQ(9, endTimePair.first); - EXPECT_EQ(721000000, endTimePair.second); + EXPECT_EQ(5, endTimePair.first); + EXPECT_EQ(800000000, endTimePair.second); } ///////////////////////////////////////////////// @@ -691,9 +692,7 @@ TEST_F(LogSystemTest, RecordAndPlayback) // Create temp directory to store log this->CreateLogsDir(); - // Used to count the expected number of poses recorded. Counting is necessary - // as the number varied depending on the CPU load. - int expectedPoseCount = 0; + int numIterations = 1000; // Record { // World with moving entities @@ -706,26 +705,9 @@ TEST_F(LogSystemTest, RecordAndPlayback) recordServerConfig.SetUseLogRecord(true); recordServerConfig.SetLogRecordPath(this->logDir); - std::chrono::steady_clock::time_point lastPoseTime; - const int poseHz = 60; - const std::chrono::duration msgPeriod{1.0 / poseHz}; - // This system counts the expected number of poses recorded by reproducing - // the throttle mechanism used by ign-transport. - test::Relay recordedPoseCounter; - recordedPoseCounter.OnPostUpdate( - [&](const UpdateInfo &, const EntityComponentManager &) - { - auto tNow = std::chrono::steady_clock::now(); - if ((tNow - lastPoseTime) > msgPeriod) - { - lastPoseTime = tNow; - ++expectedPoseCount; - } - }); - // Run for a few seconds to record different poses + // Run for a number of iterations to record different poses Server recordServer(recordServerConfig); - recordServer.AddSystem(recordedPoseCounter.systemPtr); - recordServer.Run(true, 1000, false); + recordServer.Run(true, numIterations, false); } // Verify file is created @@ -775,46 +757,6 @@ TEST_F(LogSystemTest, RecordAndPlayback) EXPECT_EQ(32, stateMsg.entities_size()); EXPECT_NE(batch.end(), ++recordedIter); - // check rest of recordIter (state message) for poses - while (batch.end() != recordedIter) - { - EXPECT_EQ("ignition.msgs.SerializedStateMap", recordedIter->Type()); - EXPECT_EQ(recordedIter->Topic(), "/world/log_pendulum/changed_state"); - - stateMsg.ParseFromString(recordedIter->Data()); - - for (const auto &entityIter : stateMsg.entities()) - { - for (const auto &compIter : entityIter.second.components()) - { - EXPECT_EQ(components::Pose::typeId, compIter.second.type()); - } - } - ++recordedIter; - } - - EXPECT_EQ(batch.end(), recordedIter); - - // Keep track of total number of pose comparisons - int nTotal{0}; - - // Check poses - batch = log.QueryMessages(transport::log::TopicPattern( - std::regex(".*/dynamic_pose/info"))); - recordedIter = batch.begin(); - EXPECT_NE(batch.end(), recordedIter); - EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type()); - - // First pose at 1ms time, both from log clock and header - EXPECT_EQ(1000000, recordedIter->TimeReceived().count()); - - msgs::Pose_V recordedMsg; - recordedMsg.ParseFromString(recordedIter->Data()); - ASSERT_TRUE(recordedMsg.has_header()); - ASSERT_TRUE(recordedMsg.header().has_stamp()); - EXPECT_EQ(0, recordedMsg.header().stamp().sec()); - EXPECT_EQ(1000000, recordedMsg.header().stamp().nsec()); - // Playback config ServerConfig playServerConfig; playServerConfig.SetLogPlaybackPath(logPlaybackDir); @@ -823,68 +765,59 @@ TEST_F(LogSystemTest, RecordAndPlayback) Server playServer(playServerConfig); // Callback function for entities played back - // Compare current pose being played back with the pose with the closest - // timestamp in the recorded file. + // Compare current pose being played back with the pose from the stateMsg test::Relay playbackPoseTester; playbackPoseTester.OnPostUpdate( - [&](const UpdateInfo &_info, const EntityComponentManager &_ecm) + [&](const UpdateInfo &, const EntityComponentManager &_ecm) { // Playback continues even after the log file is over if (batch.end() == recordedIter) return; // Get next recorded message - EXPECT_EQ("ignition.msgs.Pose_V", recordedIter->Type()); - recordedMsg.ParseFromString(recordedIter->Data()); - - ASSERT_TRUE(recordedMsg.has_header()); - ASSERT_TRUE(recordedMsg.header().has_stamp()); - EXPECT_EQ(0, recordedMsg.header().stamp().sec()); - - // Log clock timestamp matches message timestamp - EXPECT_EQ(recordedMsg.header().stamp().nsec(), - recordedIter->TimeReceived().count()); - - // A recorded messages is matched when its timestamp is within 100us - // of the current iteration's sim time. - if (std::abs((_info.simTime - recordedIter->TimeReceived()).count()) > - 100000) - { - return; - } - - // Has 6 dynamic entities: 4 in dbl pendulum and 2 in nested model - EXPECT_EQ(6, recordedMsg.pose_size()); + EXPECT_EQ("ignition.msgs.SerializedStateMap", recordedIter->Type()); + EXPECT_EQ(recordedIter->Topic(), "/world/log_pendulum/changed_state"); + stateMsg.ParseFromString(recordedIter->Data()); // Loop through all recorded poses, and check them against the // playedback poses. - for (int i = 0; i < recordedMsg.pose_size(); ++i) + for (const auto &entityIter : stateMsg.entities()) { - const math::Pose3d &poseRecorded = msgs::Convert(recordedMsg.pose(i)); - const std::string &name = recordedMsg.pose(i).name(); + Entity entity = entityIter.second.id(); + auto nameComp = _ecm.Component(entity); + ASSERT_NE(nullptr, nameComp); + const std::string &name = nameComp->Data(); - auto entity = _ecm.EntityByComponents( - components::Name(recordedMsg.pose(i).name())); - ASSERT_NE(kNullEntity, entity); auto poseComp = _ecm.Component(entity); ASSERT_NE(nullptr, poseComp); - const auto &posePlayed = poseComp->Data(); - - EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1) - << name; - EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1) - << name; - EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1) - << name; - - EXPECT_NEAR(posePlayed.Rot().Roll(), poseRecorded.Rot().Roll(), 0.1) - << name; - EXPECT_NEAR(posePlayed.Rot().Pitch(), poseRecorded.Rot().Pitch(), 0.1) - << name; - EXPECT_NEAR(posePlayed.Rot().Yaw(), poseRecorded.Rot().Yaw(), 0.1) - << name; - } + const math::Pose3d &posePlayed = poseComp->Data(); + for (const auto &compIter : entityIter.second.components()) + { + msgs::SerializedComponent compMsg = compIter.second; + ASSERT_EQ(components::Pose::typeId, compMsg.type()); + + components::Pose pose; + std::istringstream istr(compMsg.component()); + pose.Deserialize(istr); + const math::Pose3d &poseRecorded = pose.Data(); + + EXPECT_NEAR(posePlayed.Pos().X(), poseRecorded.Pos().X(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Pos().Y(), poseRecorded.Pos().Y(), 0.1) + << name; + EXPECT_NEAR(posePlayed.Pos().Z(), poseRecorded.Pos().Z(), 0.1) + << name; + + EXPECT_NEAR(posePlayed.Rot().Roll(), + poseRecorded.Rot().Roll(), 0.1) << name; + EXPECT_NEAR(posePlayed.Rot().Pitch(), + poseRecorded.Rot().Pitch(), 0.1) << name; + EXPECT_NEAR(posePlayed.Rot().Yaw(), + poseRecorded.Rot().Yaw(), 0.1) << name; + + } + } ++recordedIter; }); @@ -894,15 +827,11 @@ TEST_F(LogSystemTest, RecordAndPlayback) // checked in the playbackPoseTester playServer.Run(true, 500, false); - // Count the total number of pose messages in the log file + // Count the total number of state messages in the log file + int nTotal{0}; for (auto it = batch.begin(); it != batch.end(); ++it, ++nTotal) { } - // The expectedPoseCount might be off by ±2 because the ign transport's - // throttle mechanism (which is used by the SceneBroadcaster when publishing - // poses) uses real-time - EXPECT_LE(std::abs(nTotal - expectedPoseCount), 2) - << "nTotal [" << nTotal << "] expectedPoseCount [" << expectedPoseCount - << "]"; + EXPECT_EQ(numIterations, nTotal); this->RemoveLogsDir(); } @@ -910,6 +839,12 @@ TEST_F(LogSystemTest, RecordAndPlayback) ///////////////////////////////////////////////// TEST_F(LogSystemTest, LogControl) { + // TODO(anyone) when re-recording state.tlog file, do not run + // `ign gazebo --record rolling_shapes.sdf` with `-r` flag and pause sim + // before terminating. For some reason, when running with `-r` &/or + // terminating sim w/o pausing causing strange pose behavior + // when seeking close to end of file followed by rewind. For more details: + // https://github.com/ignitionrobotics/ign-gazebo/pull/839 auto logPath = common::joinPaths(PROJECT_SOURCE_PATH, "test", "media", "rolling_shapes_log"); @@ -952,7 +887,7 @@ TEST_F(LogSystemTest, LogControl) transport::Node node; // Seek forward (downhill) - std::vector secs(9); + std::vector secs(5); std::iota(std::begin(secs), std::end(secs), 1); msgs::LogPlaybackControl req; @@ -1675,8 +1610,8 @@ TEST_F(LogSystemTest, LogTopics) transport::log::Playback player(statePath); const int64_t addTopicResult = player.AddTopic(std::regex(".*")); - // There should be 4 topics, with the clock topic. - EXPECT_EQ(4, addTopicResult); + // There should be 3 topics (clock, sdf, & state) + EXPECT_EQ(3, addTopicResult); int clockMsgCount = 0; std::function clockCb = diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 00c8ab4fc06..e918d2885e4 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -205,6 +205,135 @@ class OdometryPublisherTest EXPECT_NEAR(odomAngVels.back().Z(), angVelCmd[2], 1e-1); } + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _odomTopic Odometry topic. + protected: void TestMovement3d(const std::string &_sdfFile, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses and velocities + test::Relay testSystem; + + std::vector poses; + testSystem.OnPostUpdate([&poses](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + auto id = _ecm.EntityByComponents( + components::Model(), + components::Name("X3")); + EXPECT_NE(kNullEntity, id); + + auto poseComp = _ecm.Component(id); + ASSERT_NE(nullptr, poseComp); + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server while model is stationary + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (const auto &pose : poses) + { + EXPECT_EQ(poses[0], pose); + } + + // 50 Hz is the default publishing freq + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::vector odomLinVels; + std::vector odomAngVels; + // Create function to store data from odometry messages + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + odomLinVels.push_back(msgs::Convert(_msg.twist().linear())); + odomAngVels.push_back(msgs::Convert(_msg.twist().angular())); + }; + // Create node for publishing twist messages + transport::Node node; + auto cmdVel = node.Advertise("/X3/gazebo/command/twist"); + node.Subscribe(_odomTopic, odomCb); + + test::Relay velocityRamp; + math::Vector3d linVelCmd(0.5, 0.3, 1.5); + math::Vector3d angVelCmd(0.0, 0.0, 0.2); + velocityRamp.OnPreUpdate( + [&](const gazebo::UpdateInfo &/*_info*/, + gazebo::EntityComponentManager &/*_ecm*/) + { + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), linVelCmd); + msgs::Set(msg.mutable_angular(), angVelCmd); + cmdVel.Publish(msg); + }); + + server.AddSystem(velocityRamp.systemPtr); + + // Run server while the model moves with the velocities set earlier + server.Run(true, 3000, false); + + // Poses for 4s + ASSERT_EQ(4000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 150 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + ASSERT_NE(maxSleep, sleep); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(150u, odomPoses.size()); + EXPECT_EQ(150u, odomLinVels.size()); + EXPECT_EQ(150u, odomAngVels.size()); + + // Check accuracy of poses published in the odometry message + auto finalModelFramePose = odomPoses.back(); + EXPECT_NEAR(poses[1020].Pos().X(), odomPoses[0].Pos().X(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Pos().Z(), odomPoses[0].Pos().Z(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().X(), odomPoses[0].Rot().X(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Y(), odomPoses[0].Rot().Y(), 1e-2); + EXPECT_NEAR(poses[1020].Rot().Z(), odomPoses[0].Rot().Z(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), finalModelFramePose.Pos().X(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Y(), finalModelFramePose.Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().Z(), finalModelFramePose.Pos().Z(), 1e-2); + EXPECT_NEAR(poses.back().Rot().X(), finalModelFramePose.Rot().X(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Y(), finalModelFramePose.Rot().Y(), 1e-2); + EXPECT_NEAR(poses.back().Rot().Z(), finalModelFramePose.Rot().Z(), 1e-2); + + // Check accuracy of velocities published in the odometry message + EXPECT_NEAR(odomLinVels.back().X(), linVelCmd[0], 1e-1); + EXPECT_NEAR(odomLinVels.back().Y(), linVelCmd[1], 1e-1); + EXPECT_NEAR(odomLinVels.back().Z(), linVelCmd[2], 1e-1); + EXPECT_NEAR(odomAngVels.back().X(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().Y(), 0.0, 1e-1); + EXPECT_NEAR(odomAngVels.back().Z(), angVelCmd[2], 1e-1); + } + /// \param[in] _sdfFile SDF file to load. /// \param[in] _odomTopic Odometry topic. /// \param[in] _frameId Name of the world-fixed coordinate frame @@ -283,6 +412,15 @@ TEST_P(OdometryPublisherTest, MovementCustomTopic) "/model/bar/odom"); } +///////////////////////////////////////////////// +TEST_P(OdometryPublisherTest, Movement3d) +{ + TestMovement3d( + ignition::common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "odometry_publisher_3d.sdf"), + "/model/X3/odometry"); +} + ///////////////////////////////////////////////// TEST_P(OdometryPublisherTest, OdomFrameId) { diff --git a/test/integration/particle_emitter2.cc b/test/integration/particle_emitter2.cc index 920f9440808..d8b4922c7bc 100644 --- a/test/integration/particle_emitter2.cc +++ b/test/integration/particle_emitter2.cc @@ -113,23 +113,9 @@ TEST_F(ParticleEmitter2Test, SDFLoad) EXPECT_EQ("/path/to/dummy_image.png", _emitter->Data().color_range_image().data()); - // particle scatter ratio is temporarily stored in header - bool hasParticleScatterRatio = false; - for (int i = 0; i < _emitter->Data().header().data_size(); ++i) - { - for (int j = 0; - j < _emitter->Data().header().data(i).value_size(); ++j) - { - if (_emitter->Data().header().data(i).key() == - "particle_scatter_ratio") - { - EXPECT_DOUBLE_EQ(0.01, math::parseFloat( - _emitter->Data().header().data(i).value(0))); - hasParticleScatterRatio = true; - } - } - } - EXPECT_TRUE(hasParticleScatterRatio); + EXPECT_TRUE(_emitter->Data().has_particle_scatter_ratio()); + EXPECT_FLOAT_EQ(0.01f, + _emitter->Data().particle_scatter_ratio().data()); } else { diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 174ac27a128..cb21e184546 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -21,8 +21,12 @@ #include #include +#ifdef HAVE_DART +#include +#endif #include #include +#include #include #include #include @@ -44,9 +48,15 @@ #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" +#include "ignition/gazebo/components/JointEffortLimitsCmd.hh" +#include "ignition/gazebo/components/JointForceCmd.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" #include "ignition/gazebo/components/JointPosition.hh" +#include "ignition/gazebo/components/JointPositionLimitsCmd.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include "ignition/gazebo/components/JointVelocity.hh" +#include "ignition/gazebo/components/JointVelocityCmd.hh" +#include "ignition/gazebo/components/JointVelocityLimitsCmd.hh" #include "ignition/gazebo/components/JointVelocityReset.hh" #include "ignition/gazebo/components/Link.hh" #include "ignition/gazebo/components/LinearVelocity.hh" @@ -56,6 +66,7 @@ #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" #include "ignition/gazebo/components/Visual.hh" #include "ignition/gazebo/components/World.hh" @@ -71,6 +82,20 @@ class PhysicsSystemFixture : public InternalFixture<::testing::Test> { }; +class PhysicsSystemFixtureWithDart6_10 : public PhysicsSystemFixture +{ + protected: void SetUp() override + { +#ifndef HAVE_DART + GTEST_SKIP(); +#elif !DART_VERSION_AT_LEAST(6, 10, 0) + GTEST_SKIP(); +#endif + + PhysicsSystemFixture::SetUp(); + } +}; + ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) { @@ -809,6 +834,381 @@ TEST_F(PhysicsSystemFixture, ResetVelocityComponent) EXPECT_NEAR(vel0, velocities[1], 0.05); } +///////////////////////////////////////////////// +/// Test joint position limit command component +TEST_F(PhysicsSystemFixtureWithDart6_10, JointPositionLimitsCommandComponent) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/revolute_joint.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + const std::string rotatingJointName{"j2"}; + + test::Relay testSystem; + + // cppcheck-suppress variableScope + size_t iteration = 0u; + + // The system is not in equilibrium at the beginning, so normally, joint j2 + // would move. For the first 50 ms, we set position limits to 1e-6 so that + // it can't freely move, and we check it did not. For the other 50 ms, we + // remove the position limit and check that the joint has moved at least by + // 1e-2. Between times 30 and 40 ms we also add a 100 N force to the joint to + // check that the limit is held even in presence of force commands. Between + // times 40 and 50 ms, we add a velocity command to check that velocity + // commands do not break the positional limit. + + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Joint *, components::Name *_name) -> bool + { + if (_name->Data() == rotatingJointName) + { + if (iteration == 0u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_EQ(nullptr, limitComp); + _ecm.CreateComponent(_entity, + components::JointPositionLimitsCmd ({{-1e-6, 1e-6}})); + _ecm.CreateComponent(_entity, components::JointPosition()); + } + else if (iteration == 50u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + limitComp->Data() = {{-1e6, 1e6}}; + } + } + else + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + EXPECT_EQ(0u, limitComp->Data().size()); + } + if (iteration >= 30u && iteration < 40u) + { + _ecm.SetComponentData( + _entity, {100.0}); + } + else if (iteration >= 40u && iteration < 50u) + { + _ecm.SetComponentData( + _entity, {1.0}); + } + } + ++iteration; + } + return true; + }); + }); + + std::vector positions; + + testSystem.OnPostUpdate([&]( + const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_pos) + { + if (_name->Data() == rotatingJointName) + { + positions.push_back(_pos->Data()[0]); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 100, false); + + ASSERT_EQ(positions.size(), 100ul); + // The 1e-6 limit is slightly overcome, but not very much + EXPECT_NEAR(positions[0], positions[20], 2e-5); + EXPECT_NEAR(positions[0], positions[30], 3e-5); + EXPECT_NEAR(positions[0], positions[40], 3e-5); + EXPECT_NEAR(positions[0], positions[49], 3e-5); + EXPECT_LT(std::abs(positions[50]) + 1e-2, std::abs(positions[99])); +} + +///////////////////////////////////////////////// +/// Test joint velocity limit command component +TEST_F(PhysicsSystemFixtureWithDart6_10, JointVelocityLimitsCommandComponent) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/revolute_joint.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + const std::string rotatingJointName{"j2"}; + + test::Relay testSystem; + + // cppcheck-suppress variableScope + size_t iteration = 0u; + + // The system is not in equilibrium at the beginning, so normally, joint j2 + // would move. For the first 50 ms, we set velocity limits to 0.1 so that + // it can't move very fast, and we check it does not. For the other 50 ms, we + // remove the velocity limit and check that the joint has moved faster. + // Between times 30 and 40 ms we also add a 100 N force to the joint to + // check that the limit is held even in presence of force commands. Between + // times 40 and 50 ms, we add a velocity command to check that velocity + // commands do not break the velocity limit. + + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Joint *, components::Name *_name) -> bool + { + if (_name->Data() == rotatingJointName) + { + if (iteration == 0u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_EQ(nullptr, limitComp); + _ecm.CreateComponent(_entity, + components::JointVelocityLimitsCmd ({{-0.1, 0.1}})); + _ecm.CreateComponent(_entity, components::JointVelocity()); + } + else if (iteration == 50u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + limitComp->Data() = {{-1e6, 1e6}}; + } + } + else + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + EXPECT_EQ(0u, limitComp->Data().size()); + } + if (iteration >= 30u && iteration < 40u) + { + _ecm.SetComponentData( + _entity, {100.0}); + } + else if (iteration >= 40u && iteration < 50u) + { + _ecm.SetComponentData( + _entity, {1.0}); + } + } + ++iteration; + } + return true; + }); + }); + + std::vector velocities; + + testSystem.OnPostUpdate([&]( + const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointVelocity *_vel) + { + if (_name->Data() == rotatingJointName) + { + velocities.push_back(_vel->Data()[0]); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 100, false); + + ASSERT_EQ(velocities.size(), 100ul); + // The 0.1 limit is slightly overcome, but not very much + EXPECT_NEAR(0.1, velocities[20], 1e-2); + EXPECT_NEAR(0.1, velocities[30], 1e-2); + EXPECT_NEAR(0.1, velocities[40], 1e-2); + EXPECT_NEAR(0.1, velocities[49], 1e-2); + EXPECT_LT(0.5, std::abs(velocities[99])); +} + + +///////////////////////////////////////////////// +/// Test joint effort limit command component +TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) +{ + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/revolute_joint_equilibrium.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + const std::string rotatingJointName{"j2"}; + + test::Relay testSystem; + + // cppcheck-suppress variableScope + size_t iteration = 0u; + + // The system is in equilibrium at the beginning. + // For the first 50 ms, we set effort limits to 1e-6 so that + // it can't move, and we check it does not. For the other 50 ms, we + // remove the effort limit and check that the joint has moved. + // Between times 30 and 40 ms we also add a 100 N force to the joint to + // check that the limit is held even in presence of force commands. Between + // times 40 and 50 ms, we add a velocity command to check that velocity + // commands do not break the effort limit. + + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Joint *, components::Name *_name) -> bool + { + if (_name->Data() == rotatingJointName) + { + if (iteration == 0u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_EQ(nullptr, limitComp); + _ecm.CreateComponent(_entity, + components::JointEffortLimitsCmd ({{-1e-6, 1e-6}})); + _ecm.CreateComponent(_entity, components::JointPosition()); + } + else if (iteration == 50u) + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + limitComp->Data() = {{-1e9, 1e9}}; + } + } + else + { + auto limitComp = + _ecm.Component(_entity); + EXPECT_NE(nullptr, limitComp); + if (limitComp) + { + EXPECT_EQ(0u, limitComp->Data().size()); + } + if (iteration >= 30u && iteration < 40u) + { + _ecm.SetComponentData( + _entity, {100.0}); + } + else if (iteration >= 40u && iteration < 50u) + { + _ecm.SetComponentData( + _entity, {1.0}); + } + else if (iteration >= 50u) + { + _ecm.Component(_entity)->Data() = + {1000.0}; + } + } + ++iteration; + } + return true; + }); + }); + + std::vector positions; + + testSystem.OnPostUpdate([&]( + const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_pos) + { + if (_name->Data() == rotatingJointName) + { + positions.push_back(_pos->Data()[0]); + } + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 100, false); + + ASSERT_EQ(positions.size(), 100ul); + // The 1e-6 limit is slightly overcome, but not very much + EXPECT_NEAR(positions[0], positions[20], 2e-5); + EXPECT_NEAR(positions[0], positions[30], 3e-5); + EXPECT_NEAR(positions[0], positions[40], 3e-5); + EXPECT_NEAR(positions[0], positions[49], 3e-5); + EXPECT_LT(std::abs(positions[50]) + 1e-2, std::abs(positions[99])); +} + ///////////////////////////////////////////////// TEST_F(PhysicsSystemFixture, GetBoundingBox) { @@ -1662,3 +2062,124 @@ TEST_F(PhysicsSystemFixture, Heightmap) EXPECT_TRUE(checked); EXPECT_EQ(1000, maxIt); } + +///////////////////////////////////////////////// +// Joint force +TEST_F(PhysicsSystemFixture, JointTransmittedWrench) +{ + common::Console::SetVerbosity(4); + ignition::gazebo::ServerConfig serverConfig; + + const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/joint_transmitted_wrench.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + gazebo::Server server(serverConfig); + + server.SetUpdatePeriod(1us); + + // Create a system that records the poses of the links after physics + test::Relay testSystem; + + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &_info, gazebo::EntityComponentManager &_ecm) + { + if (_info.iterations == 1) + { + _ecm.Each( + [&](const ignition::gazebo::Entity &_entity, + const components::Joint *) -> bool + { + _ecm.CreateComponent(_entity, + components::JointTransmittedWrench()); + return true; + }); + } + }); + + const std::size_t totalIters = 1800; + std::vector wrenches; + wrenches.reserve(totalIters); + // Simply collect joint wrenches. We check the values later. + testSystem.OnPostUpdate( + [&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + const auto sensorJointEntity = _ecm.EntityByComponents( + components::Joint(), components::Name("sensor_joint")); + const auto jointWrench = + _ecm.ComponentData( + sensorJointEntity); + if (jointWrench.has_value()) + { + wrenches.push_back(*jointWrench); + } + }); + server.AddSystem(testSystem.systemPtr); + server.Run(true, totalIters, false); + + ASSERT_EQ(totalIters, wrenches.size()); + + const double kWeightScaleContactHeight = 0.05 + 0.25; + const double kSensorMass = 0.2; + const double kWeightMass = 10; + const double kGravity = 9.8; + const double kWeightInitialHeight = 0.5; + const double dt = 0.001; + const double timeOfContact = std::sqrt( + 2 * (kWeightInitialHeight - kWeightScaleContactHeight) / kGravity); + std::size_t iterOfContact = + static_cast(std::round(timeOfContact / dt)); + + for (std::size_t i = 0; i < iterOfContact - 10; ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force().x(), 1e-3); + EXPECT_NEAR(0.0, wrench.force().y(), 1e-3); + EXPECT_NEAR(kGravity * kSensorMass, wrench.force().z(), 1e-3); + EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque())); + } + + // Wait 300 (determined empirically) iterations for values to stabilize. + for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force().x(), 1e-3); + EXPECT_NEAR(0.0, wrench.force().y(), 1e-3); + EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force().z(), + 1e-3); + EXPECT_EQ(math::Vector3d::Zero, msgs::Convert(wrench.torque())); + } + + // Move the weight off center so it generates torque + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + const auto weightEntity = _ecm.EntityByComponents( + components::Model(), components::Name("weight")); + _ecm.SetComponentData( + weightEntity, math::Pose3d(0.2, 0.1, 0.5, 0, 0, 0)); + }); + + server.RunOnce(); + // Reset PreUpdate so it doesn't keep moving the weight + testSystem.OnPreUpdate({}); + wrenches.clear(); + server.Run(true, totalIters, false); + ASSERT_EQ(totalIters, wrenches.size()); + + // Wait 300 (determined empirically) iterations for values to stabilize. + for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force().x(), 1e-3); + EXPECT_NEAR(0.0, wrench.force().y(), 1e-3); + EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force().z(), + 1e-3); + + EXPECT_NEAR(0.1 * kGravity * kWeightMass, wrench.torque().x(), 1e-3); + EXPECT_NEAR(-0.2 * kGravity * kWeightMass, wrench.torque().y(), 1e-3); + EXPECT_NEAR(0.0, wrench.torque().z(), 1e-3); + } +} diff --git a/test/integration/spherical_coordinates.cc b/test/integration/spherical_coordinates.cc new file mode 100644 index 00000000000..aff21645c72 --- /dev/null +++ b/test/integration/spherical_coordinates.cc @@ -0,0 +1,339 @@ +/* + * 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 +#include +#include + +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.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/SphericalCoordinates.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/EnvTestFixture.hh" +#include "../helpers/Relay.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +/// \brief Test SphericalCoordinates system +class SphericalCoordinatesTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, InitialFromSDF) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + auto scComp = _ecm.Component(entity); + EXPECT_NE(nullptr, scComp); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + int expectedIterations{10}; + fixture.Server()->Run(true, expectedIterations, false); + + // Check values from SDF + EXPECT_DOUBLE_EQ(expectedIterations, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(-22.9, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(-43.2, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(0.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(0.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, SetWorldOriginFromTransport) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + // Set through transport and check + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(35.6); + req.set_longitude_deg(140.1); + req.set_elevation(456.0); + req.set_heading_deg(-20.0); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/set_spherical_coordinates"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; latest.LatitudeReference().Degree() < 1.0 && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + EXPECT_DOUBLE_EQ(expectedIterations, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(35.6, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(140.1, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(456.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(-20.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, SetWorldOriginFromComponent) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + math::SphericalCoordinates latest; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + latest = world.SphericalCoordinates(_ecm).value(); + + iterations++; + }).Finalize(); + + // Set throught C++ API and check + fixture.OnPreUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + ignition::gazebo::EntityComponentManager &_ecm) + { + auto entity = worldEntity(_ecm); + EXPECT_NE(kNullEntity, entity); + + World world(entity); + world.SetSphericalCoordinates(_ecm, math::SphericalCoordinates( + math::SphericalCoordinates::EARTH_WGS84, IGN_DTOR(52.2), + IGN_DTOR(21.0), 789.0, 0)); + }); + + fixture.Server()->Run(true, 1, false); + EXPECT_DOUBLE_EQ(1, iterations); + EXPECT_DOUBLE_EQ(math::SphericalCoordinates::EARTH_WGS84, latest.Surface()); + EXPECT_DOUBLE_EQ(52.2, latest.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(21.0, latest.LongitudeReference().Degree()); + EXPECT_DOUBLE_EQ(789.0, latest.ElevationReference()); + EXPECT_DOUBLE_EQ(0.0, latest.HeadingOffset().Degree()); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, MoveEntity) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + Entity modelEntity{kNullEntity}; + math::SphericalCoordinates worldLatLon; + math::Vector3d modelLatLon; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + World world(worldEntity(_ecm)); + + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + worldLatLon = world.SphericalCoordinates(_ecm).value(); + + modelEntity = _ecm.EntityByComponents(components::Model(), + components::Name("north")); + auto modelCoord = sphericalCoordinates(modelEntity, _ecm); + EXPECT_TRUE(modelCoord); + modelLatLon = modelCoord.value(); + + iterations++; + }).Finalize(); + + // An entity on +Y (North) has higher Latitude than the origin + fixture.Server()->Run(true, 1, false); + EXPECT_NE(kNullEntity, modelEntity); + EXPECT_GT(modelLatLon.X(), worldLatLon.LatitudeReference().Degree()); + EXPECT_DOUBLE_EQ(modelLatLon.Y(), worldLatLon.LongitudeReference().Degree()); + + // Move entity through transport and check + double desiredLat{-23.0}; + double desiredLon{-43.3}; + msgs::SphericalCoordinates req; + req.set_surface_model(msgs::SphericalCoordinates::EARTH_WGS84); + req.set_latitude_deg(desiredLat); + req.set_longitude_deg(desiredLon); + auto entityMsg = req.mutable_entity(); + entityMsg->set_id(modelEntity); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/set_spherical_coordinates"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; modelLatLon.X() > worldLatLon.LatitudeReference().Degree() + && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + + EXPECT_NEAR(modelLatLon.X(), desiredLat, 1e-6); + EXPECT_NEAR(modelLatLon.Y(), desiredLon, 1e-6); +} + +///////////////////////////////////////////////// +TEST_F(SphericalCoordinatesTest, CreateEntity) +{ + TestFixture fixture(std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/spherical_coordinates.sdf"); + + int iterations{0}; + Entity modelEntity{kNullEntity}; + math::SphericalCoordinates worldLatLon; + math::Vector3d modelLatLon; + fixture.OnPostUpdate( + [&]( + const ignition::gazebo::UpdateInfo &, + const ignition::gazebo::EntityComponentManager &_ecm) + { + World world(worldEntity(_ecm)); + + EXPECT_TRUE(world.SphericalCoordinates(_ecm)); + worldLatLon = world.SphericalCoordinates(_ecm).value(); + + // Get model once it's spawned + modelEntity = _ecm.EntityByComponents(components::Model(), + components::Name("spawned")); + if (kNullEntity != modelEntity) + { + auto modelCoord = sphericalCoordinates(modelEntity, _ecm); + EXPECT_TRUE(modelCoord); + modelLatLon = modelCoord.value(); + } + + iterations++; + }).Finalize(); + + // Create entity at spherical coordinates + auto modelStr = std::string("") + + "" + + "" + + "" + + "" + + "" + + ""; + + double desiredLat{-23.0}; + double desiredLon{-43.3}; + + msgs::EntityFactory req; + req.set_sdf(modelStr); + + auto scMsg = req.mutable_spherical_coordinates(); + scMsg->set_latitude_deg(desiredLat); + scMsg->set_longitude_deg(desiredLon); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/spherical_coordinates/create"}; + + transport::Node node; + EXPECT_TRUE(node.Request(service, req, timeout, res, result)); + EXPECT_TRUE(result); + EXPECT_TRUE(res.data()); + + int sleep{0}; + int maxSleep{30}; + int expectedIterations{0}; + for (; kNullEntity == modelEntity && sleep < maxSleep; sleep++) + { + fixture.Server()->Run(true, 1, false); + expectedIterations++; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_NE(maxSleep, sleep); + EXPECT_NE(kNullEntity, modelEntity); + EXPECT_NEAR(modelLatLon.X(), desiredLat, 1e-6); + EXPECT_NEAR(modelLatLon.Y(), desiredLon, 1e-6); +} diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc new file mode 100644 index 00000000000..591bad25806 --- /dev/null +++ b/test/integration/thruster.cc @@ -0,0 +1,216 @@ +/* + * 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/Link.hh" +#include "ignition/gazebo/Model.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/SystemLoader.hh" +#include "ignition/gazebo/TestFixture.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/World.hh" + +#include "ignition/gazebo/test_config.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace ignition; +using namespace gazebo; + +class ThrusterTest : public InternalFixture<::testing::Test> +{ + /// \brief Test a world file + /// \param[in] _world Path to world file + /// \param[in] _namespace Namespace for topic + /// \param[in] _coefficient Thrust coefficient + /// \param[in] _density Fluid density + /// \param[in] _diameter Propeller diameter + /// \param[in] _baseTol Base tolerance for most quantities + public: void TestWorld(const std::string &_world, + const std::string &_namespace, double _coefficient, double _density, + double _diameter, double _baseTol); +}; + +////////////////////////////////////////////////// +void ThrusterTest::TestWorld(const std::string &_world, + const std::string &_namespace, double _coefficient, double _density, + double _diameter, double _baseTol) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_world); + + TestFixture fixture(serverConfig); + + Model model; + Link propeller; + std::vector modelPoses; + std::vector propellerAngVels; + double dt{0.0}; + fixture. + OnConfigure( + [&](const ignition::gazebo::Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + ignition::gazebo::EntityComponentManager &_ecm, + ignition::gazebo::EventManager &/*_eventMgr*/) + { + World world(_worldEntity); + + auto modelEntity = world.ModelByName(_ecm, "sub"); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto propellerEntity = model.LinkByName(_ecm, "propeller"); + EXPECT_NE(propellerEntity, kNullEntity); + + propeller = Link(propellerEntity); + propeller.EnableVelocityChecks(_ecm); + }). + OnPostUpdate([&](const gazebo::UpdateInfo &_info, + const gazebo::EntityComponentManager &_ecm) + { + dt = std::chrono::duration(_info.dt).count(); + + auto modelPose = worldPose(model.Entity(), _ecm); + modelPoses.push_back(modelPose); + + auto propellerAngVel = propeller.WorldAngularVelocity(_ecm); + ASSERT_TRUE(propellerAngVel); + propellerAngVels.push_back(propellerAngVel.value()); + }). + Finalize(); + + // Check initial position + fixture.Server()->Run(true, 100, false); + EXPECT_EQ(100u, modelPoses.size()); + EXPECT_EQ(100u, propellerAngVels.size()); + + EXPECT_NE(model.Entity(), kNullEntity); + EXPECT_NE(propeller.Entity(), kNullEntity); + + for (const auto &pose : modelPoses) + { + EXPECT_EQ(math::Pose3d(), pose); + } + modelPoses.clear(); + for (const auto &vel : propellerAngVels) + { + EXPECT_EQ(math::Vector3d::Zero, vel); + } + propellerAngVels.clear(); + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise( + "/model/" + _namespace + "/joint/propeller_joint/cmd_thrust"); + + int sleep{0}; + int maxSleep{30}; + for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_TRUE(pub.HasConnections()); + + double force{300.0}; + msgs::Double msg; + msg.set_data(force); + pub.Publish(msg); + + // Check movement + for (sleep = 0; modelPoses.back().Pos().X() < 5.0 && sleep < maxSleep; + ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + fixture.Server()->Run(true, 100, false); + } + EXPECT_LT(sleep, maxSleep); + EXPECT_LT(5.0, modelPoses.back().Pos().X()); + + EXPECT_EQ(100u * sleep, modelPoses.size()); + EXPECT_EQ(100u * sleep, propellerAngVels.size()); + + // F = m * a + // s = a * t^2 / 2 + // F = m * 2 * s / t^2 + // s = F * t^2 / 2m + double mass{100.1}; + double xTol{1e-2}; + for (unsigned int i = 0; i < modelPoses.size(); ++i) + { + auto pose = modelPoses[i]; + auto time = dt * i; + EXPECT_NEAR(force * time * time / (2 * mass), pose.Pos().X(), xTol); + EXPECT_NEAR(0.0, pose.Pos().Y(), _baseTol); + EXPECT_NEAR(0.0, pose.Pos().Z(), _baseTol); + EXPECT_NEAR(0.0, pose.Rot().Pitch(), _baseTol); + EXPECT_NEAR(0.0, pose.Rot().Yaw(), _baseTol); + + // The joint velocity command adds some roll to the body which the PID + // wrench doesn't + if (_namespace == "custom") + EXPECT_NEAR(0.0, pose.Rot().Roll(), 0.1); + else + EXPECT_NEAR(0.0, pose.Rot().Roll(), _baseTol); + } + + // See Thor I Fossen's "Guidance and Control of ocean vehicles" p. 246 + // omega = sqrt(thrust / + // (fluid_density * thrust_coefficient * propeller_diameter ^ 4)) + auto omega = sqrt(force / (_density * _coefficient * pow(_diameter, 4))); + double omegaTol{1e-1}; + for (unsigned int i = 0; i < propellerAngVels.size(); ++i) + { + auto angVel = propellerAngVels[i]; + // It takes a few iterations to reach the speed + if (i > 25) + { + EXPECT_NEAR(omega, angVel.X(), omegaTol) << i; + } + EXPECT_NEAR(0.0, angVel.Y(), _baseTol); + EXPECT_NEAR(0.0, angVel.Z(), _baseTol); + } +} + +///////////////////////////////////////////////// +TEST_F(ThrusterTest, PIDControl) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_pid.sdf"); + + // Tolerance could be lower (1e-6) if the joint pose had a precise 180 + // rotation + this->TestWorld(world, "sub", 0.004, 1000, 0.2, 1e-4); +} + +///////////////////////////////////////////////// +TEST_F(ThrusterTest, VelocityControl) +{ + auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "thruster_vel_cmd.sdf"); + + // Tolerance is high because the joint command disturbs the vehicle body + this->TestWorld(world, "custom", 0.005, 950, 0.25, 1e-2); +} + diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc new file mode 100644 index 00000000000..ea53742d3b1 --- /dev/null +++ b/test/integration/tracked_vehicle_system.cc @@ -0,0 +1,582 @@ +/* + * 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/PhysicsEnginePlugin.hh" +#include +#include +#include +#include +#include +#include + +#include "ignition/gazebo/components/Name.hh" +#include "ignition/gazebo/components/Model.hh" +#include "ignition/gazebo/components/Pose.hh" +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +#define tol 10e-4 + +using namespace ignition; +using namespace gazebo; +using namespace std::chrono_literals; + +#define EXPECT_ANGLE_NEAR(a1, a2, tol) \ + EXPECT_LT(std::abs(math::Angle((a1) - (a2)).Normalized().Radian()), (tol)) \ + << (a1) << " vs. " << (a2) + +// Verify that a model's world pose is near a specified pose. +void verifyPose(const math::Pose3d& pose1, const math::Pose3d& pose2) +{ + EXPECT_NEAR(pose1.Pos().X(), pose2.Pos().X(), 1e-1); + EXPECT_NEAR(pose1.Pos().Y(), pose2.Pos().Y(), 1e-1); + EXPECT_NEAR(pose1.Pos().Z(), pose2.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Roll(), pose2.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Pitch(), pose2.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(pose1.Rot().Yaw(), pose2.Rot().Yaw(), 1e-1); +} + +/// \brief Test TrackedVehicle system. This test drives a tracked robot over a +/// course of obstacles and verifies that it is able to climb on/over them. +class TrackedVehicleTest : public InternalFixture<::testing::Test> +{ + public: void SkipTestIfNotSupported(const EntityComponentManager &_ecm, + bool &_shouldSkip) + { +#if __APPLE__ + // until https://github.com/ignitionrobotics/ign-gazebo/issues/806 is fixed + _shouldSkip = true; +#else + _shouldSkip = false; + auto pluginLib = + _ecm.ComponentData(worldEntity(_ecm)); + ASSERT_TRUE(pluginLib.has_value()) + << "PhysicsEnginePlugin component not found"; + + // Find physics plugin (copied from the Physics system with some + // modifications) + common::SystemPaths systemPaths; + systemPaths.SetPluginPathEnv("IGN_GAZEBO_PHYSICS_ENGINE_PATH"); + systemPaths.AddPluginPaths({IGNITION_PHYSICS_ENGINE_INSTALL_DIR}); + + auto pathToLib = systemPaths.FindSharedLibrary(*pluginLib); + ASSERT_FALSE(pathToLib.empty()) + << "Failed to find plugin [" << *pluginLib << "]"; + + // Load engine plugin + ignition::plugin::Loader pluginLoader; + auto plugins = pluginLoader.LoadLib(pathToLib); + ASSERT_FALSE(plugins.empty()) + << "Unable to load the [" << pathToLib << "] library"; + + // Check that we do have a valid physics engine. Otherwise, this should be a + // failure not a skip. + auto classNames = pluginLoader.PluginsImplementing< + physics::ForwardStep::Implementation< + physics::FeaturePolicy3d>>(); + ASSERT_FALSE(classNames.empty()) + << "No physics plugins found in library [" << pathToLib << "]"; + + // Check if there are any plugins implementing + // SetContactPropertiesCallbackFeature. If not, skip the test. + auto contactProperties = pluginLoader.PluginsImplementing< + physics::SetContactPropertiesCallbackFeature::Implementation< + physics::FeaturePolicy3d>>(); + if (contactProperties.empty()) + { + _shouldSkip = true; + } +#endif + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + /// \param[in] _odomTopic Odometry topic. + protected: void TestPublishCmd(const std::string &_sdfFile, + const std::string &_cmdVelTopic, + const std::string &_odomTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay ecmGetterSystem; + EntityComponentManager* ecm {nullptr}; + ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (ecm == nullptr) + ecm = &_ecm; + }); + server.AddSystem(ecmGetterSystem.systemPtr); + // Get ECM + server.Run(true, 1, false); + + ASSERT_NE(nullptr, ecm); + bool shouldSkipTest = false; + this->SkipTestIfNotSupported(*ecm, shouldSkipTest); + if (shouldSkipTest) + { + // Skip test if the ContactProperties feature is not available + GTEST_SKIP() << "Skipping test because physics engine does not support " + "SetContactPropertiesCallbackFeature"; + } + + test::Relay testSystem; + Entity modelEntity {kNullEntity}; + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + modelEntity = _ecm.EntityByComponents( + components::Model(), + components::Name("simple_tracked")); + EXPECT_NE(kNullEntity, modelEntity); + + auto poseComp = _ecm.Component(modelEntity); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + // Run server and check that vehicle didn't move + server.Run(true, 1000, false); + + EXPECT_EQ(1000u, poses.size()); + + for (size_t i = 101; i < poses.size(); ++i) + { + SCOPED_TRACE(i); + verifyPose(poses[100], poses[i]); + } + + poses.clear(); + + // Get odometry messages + double period{1.0 / 50.0}; + double lastMsgTime{1.0}; + std::vector odomPoses; + std::function odomCb = + [&](const msgs::Odometry &_msg) + { + ASSERT_TRUE(_msg.has_header()); + ASSERT_TRUE(_msg.header().has_stamp()); + + double msgTime = + static_cast(_msg.header().stamp().sec()) + + static_cast(_msg.header().stamp().nsec()) * 1e-9; + + EXPECT_DOUBLE_EQ(msgTime, lastMsgTime + period); + lastMsgTime = msgTime; + + odomPoses.push_back(msgs::Convert(_msg.pose())); + }; + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + node.Subscribe(_odomTopic, odomCb); + + msgs::Twist msg; + msg.mutable_linear()->set_x(1.0); + + pub.Publish(msg); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + int sleep = 0; + int maxSleep = 30; + for (; odomPoses.size() < 50 && sleep < maxSleep; ++sleep) + { + std::this_thread::sleep_for(100ms); + } + ASSERT_NE(maxSleep, sleep); + + // Odom for 3s + ASSERT_FALSE(odomPoses.empty()); + EXPECT_EQ(50u, odomPoses.size()); + + EXPECT_LT(poses[0].Pos().X(), poses[999].Pos().X()); + EXPECT_NEAR(poses[0].Pos().Y(), poses[999].Pos().Y(), tol); + EXPECT_NEAR(poses[0].Pos().Z(), poses[999].Pos().Z(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().X(), poses[999].Rot().X(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().Y(), poses[999].Rot().Y(), tol); + EXPECT_ANGLE_NEAR(poses[0].Rot().Z(), poses[999].Rot().Z(), tol); + + // The robot starts at (3,0,0), so odom will have this shift. + EXPECT_NEAR(poses[0].Pos().X(), odomPoses[0].Pos().X() + 3.0, 3e-2); + EXPECT_NEAR(poses[0].Pos().Y(), odomPoses[0].Pos().Y(), 1e-2); + EXPECT_NEAR(poses.back().Pos().X(), odomPoses.back().Pos().X() + 3, 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), odomPoses.back().Pos().Y(), 1e-2); + + // Max velocities/accelerations expectations. + // Moving time. + double t = 1.0; + double d = poses[999].Pos().Distance(poses[0].Pos()); + double v = d / t; + EXPECT_LT(v, 1); + + poses.clear(); + + gazebo::Model model(modelEntity); + + // Move the robot somewhere to free space without obstacles. + model.SetWorldPoseCmd(*ecm, math::Pose3d(10, 10, 0.1, 0, 0, 0)); + + // Let the models settle down. + server.Run(true, 300, false); + + // Test straight driving - 1 sec driving, should move 1 meter forward. + + const auto startPose = poses.back(); + + const double linearSpeed = 1.0; + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), startPose.Pos().X() + linearSpeed, 0.1); + EXPECT_NEAR(poses.back().Pos().Y(), startPose.Pos().Y(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), startPose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), startPose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Pitch(), startPose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), startPose.Rot().Yaw(), 1e-1); + + // Test rotation in place - 1 sec rotation, should turn 0.25 rad. + + const auto middlePose = poses.back(); + + // Take care when changing this value - if too high, it could get restricted + // by the max speed of the tracks. + const double rotationSpeed = 0.25; + msgs::Set(msg.mutable_linear(), math::Vector3d(0, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, rotationSpeed)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), middlePose.Pos().X(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), middlePose.Pos().Y(), 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), middlePose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), middlePose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Pitch(), middlePose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), + middlePose.Rot().Yaw() + rotationSpeed, 1e-1); + + // Test following a circular path. + + const auto lastPose = poses.back(); + + msgs::Set(msg.mutable_linear(), math::Vector3d(0.5, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0.2)); + pub.Publish(msg); + server.Run(true, 1000, false); + + EXPECT_NEAR(poses.back().Pos().X(), lastPose.Pos().X() + 0.4, 1e-1); + EXPECT_NEAR(poses.back().Pos().Y(), lastPose.Pos().Y() + 0.15, 1e-1); + EXPECT_NEAR(poses.back().Pos().Z(), lastPose.Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), lastPose.Rot().Roll(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), lastPose.Rot().Pitch(), 1e-2); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), lastPose.Rot().Yaw() + 0.2, 1e-1); + + // Test driving on staircase - should climb to its middle part. + + const auto beforeStairsPose = math::Pose3d( + 3, 0, 0.1, + 0, 0, 0); + model.SetWorldPoseCmd(*ecm, beforeStairsPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 3500, false); + + EXPECT_NEAR(poses.back().Pos().X(), beforeStairsPose.X() + 3.4, 0.15); + EXPECT_LE(poses.back().Pos().Y(), 0.7); + EXPECT_GT(poses.back().Pos().Z(), 0.6); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), -0.4, 1e-1); + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeStairsPose.Rot().Yaw(), 1e-1); + + // Test driving over a cylinder + + const auto beforeCylinderPose = math::Pose3d( + 1, 0, 0.1, + 0, 0, -math::Angle::Pi.Radian()); + model.SetWorldPoseCmd(*ecm, beforeCylinderPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 2000, false); + + // The cylinder is at (0, 0, 0), we start at (0, 1, 0), and want to pass + // at least a bit behind the cylinder (0, -1, 0). The driving is a bit wild, + // so we don't care much about the end Y position and yaw. + EXPECT_LT(poses.back().Pos().X(), -0.99); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Y(), 0, 0.5); + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeCylinderPose.Rot().Yaw(), 0.5); + + // Test driving over an obstacle that requires flippers. Without them, the + // robot would get stuck in front of the obstacle. + + const auto beforeBoxPose = math::Pose3d( + 1, 2, 0.1, + 0, 0, -math::Angle::Pi.Radian()); + model.SetWorldPoseCmd(*ecm, beforeBoxPose); + + // Let the model settle down. + server.Run(true, 300, false); + + // we go backwards because we have the CoG in the back + msgs::Set(msg.mutable_linear(), math::Vector3d(-linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 4000, false); + + // The box is at (2, 2, 0), we start at (1, 2, 0), and want to pass + // at least a bit behind the box (3.5, 2, 0). The driving is a bit wild. + EXPECT_GT(poses.back().Pos().X(), 3.5); + EXPECT_NEAR(poses.back().Pos().Y(), 2, 0.1); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeBoxPose.Rot().Yaw(), 0.25); + // And we go back, which is a somewhat easier way + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 4000, false); + + // We start at (3.5, 2, 0), we go back, and it should be a bit faster than + // the previous traversal, so we should end up beyond the starting point. + EXPECT_LT(poses.back().Pos().X(), 1); + EXPECT_NEAR(poses.back().Pos().Y(), 2, 0.1); // The driving is wild + EXPECT_NEAR(poses.back().Pos().Z(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0.0, 1e-1); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0.0, 1e-1); + // The driving is wild + EXPECT_ANGLE_NEAR( + poses.back().Rot().Yaw(), beforeBoxPose.Rot().Yaw(), 0.25); + + // Test that disabling the contact surface customization makes the vehicle + // immobile + + ecm->Each( + [&](const Entity & _entity, + const components::Collision */*_collision*/) -> bool + { + ecm->SetComponentData( + _entity, false); + return true; + }); + + model.SetWorldPoseCmd(*ecm, beforeCylinderPose); + + // Let the model settle down. + server.Run(true, 300, false); + + msgs::Set(msg.mutable_linear(), math::Vector3d(linearSpeed, 0, 0)); + msgs::Set(msg.mutable_angular(), math::Vector3d(0, 0, 0)); + pub.Publish(msg); + server.Run(true, 500, false); + + // Verify that the vehicle did not move + poses.back().SetZ(beforeCylinderPose.Pos().Z()); // ignore Z offset + verifyPose(poses.back(), beforeCylinderPose); + } + + /// \param[in] _sdfFile SDF file to load. + /// \param[in] _cmdVelTopic Command velocity topic. + protected: void TestConveyor(const std::string &_sdfFile, + const std::string &_cmdVelTopic) + { + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(_sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Create a system that records the vehicle poses + test::Relay ecmGetterSystem; + EntityComponentManager* ecm {nullptr}; + ecmGetterSystem.OnPreUpdate([&ecm](const gazebo::UpdateInfo &, + gazebo::EntityComponentManager &_ecm) + { + if (ecm == nullptr) + ecm = &_ecm; + }); + server.AddSystem(ecmGetterSystem.systemPtr); + // Get ECM + server.Run(true, 1, false); + + ASSERT_NE(nullptr, ecm); + bool shouldSkipTest = false; + this->SkipTestIfNotSupported(*ecm, shouldSkipTest); + if (shouldSkipTest) + { + // Skip test if the ContactProperties feature is not available + GTEST_SKIP() << "Skipping test because physics engine does not support " + "SetContactPropertiesCallbackFeature"; + } + + test::Relay testSystem; + Entity boxEntity {kNullEntity}; + std::vector poses; + testSystem.OnPostUpdate([&](const gazebo::UpdateInfo &, + const gazebo::EntityComponentManager &_ecm) + { + boxEntity = _ecm.EntityByComponents( + components::Model(), + components::Name("box")); + EXPECT_NE(kNullEntity, boxEntity); + + auto poseComp = _ecm.Component(boxEntity); + ASSERT_NE(nullptr, poseComp); + + poses.push_back(poseComp->Data()); + }); + server.AddSystem(testSystem.systemPtr); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + // check that the box has not moved in X and Y directions (it will move in + // Z as it falls down on the conveyor) + EXPECT_NEAR(poses[0].Pos().X(), poses.back().Pos().X(), 1e-6); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-6); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + poses.clear(); + + // Publish command and check that vehicle moved + transport::Node node; + auto pub = node.Advertise(_cmdVelTopic); + + // In this test, there is a long conveyor and a small box at its center. + // The conveyor has max_velocity 0.5, max_acceleration 0.25 and command + // timeout 2 seconds. So we expect the box will move slowly in the first + // second, it will reach 0.5 meters in 2 seconds, and it will slow down + // for the third second (deceleration is limited even for command timeout). + + msgs::Double msg; + msg.set_data(1.0); + + pub.Publish(msg); + + server.Run(true, 1000, false); + + // Poses for 1s + ASSERT_EQ(1000u, poses.size()); + + EXPECT_NEAR(0.125, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + server.Run(true, 1000, false); + + // Poses for 2s + ASSERT_EQ(2000u, poses.size()); + + EXPECT_NEAR(0.5, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + server.Run(true, 1000, false); + + // Poses for 3s + ASSERT_EQ(3000u, poses.size()); + + EXPECT_NEAR(0.875, poses.back().Pos().X(), 1e-1); + EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); + EXPECT_NEAR(poses[0].Pos().Z(), poses.back().Pos().Z(), 1e-2); + EXPECT_ANGLE_NEAR(poses.back().Rot().Roll(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); + EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + + poses.clear(); + } +}; + +///////////////////////////////////////////////// +TEST_F(TrackedVehicleTest, PublishCmd) +{ + this->TestPublishCmd( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/tracked_vehicle_simple.sdf", + "/model/simple_tracked/cmd_vel", + "/model/simple_tracked/odometry"); +} + +///////////////////////////////////////////////// +TEST_F(TrackedVehicleTest, Conveyor) +{ + this->TestConveyor( + std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/conveyor.sdf", + "/model/conveyor/link/base_link/track_cmd_vel"); +} diff --git a/test/integration/user_commands.cc b/test/integration/user_commands.cc index 89d41d2a141..47cccb8354a 100644 --- a/test/integration/user_commands.cc +++ b/test/integration/user_commands.cc @@ -108,8 +108,6 @@ TEST_F(UserCommandsTest, Create) "" + "" + "" + - "" + - "" + ""; auto badStr = std::string("") + diff --git a/test/integration/world.cc b/test/integration/world.cc index 9657d53eb77..9e0147924c0 100644 --- a/test/integration/world.cc +++ b/test/integration/world.cc @@ -22,6 +22,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,7 @@ #include #include #include +#include #include #include #include @@ -111,6 +113,24 @@ TEST_F(WorldIntegrationTest, Atmosphere) EXPECT_EQ(math::Temperature(288.15), atmosphere.Temperature()); } +////////////////////////////////////////////////// +TEST_F(WorldIntegrationTest, SphericalCoordinates) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::World()); + + World world(id); + + EXPECT_EQ(std::nullopt, world.SphericalCoordinates(ecm)); + + world.SetSphericalCoordinates(ecm, math::SphericalCoordinates()); + + auto sphericalCoordinates = world.SphericalCoordinates(ecm).value(); + EXPECT_DOUBLE_EQ(0.0, sphericalCoordinates.LatitudeReference().Degree()); +} + ////////////////////////////////////////////////// TEST_F(WorldIntegrationTest, Gravity) { diff --git a/test/media/rolling_shapes_log/state.tlog b/test/media/rolling_shapes_log/state.tlog index 821f9558c68..0a0502e0d3f 100644 Binary files a/test/media/rolling_shapes_log/state.tlog and b/test/media/rolling_shapes_log/state.tlog differ diff --git a/test/performance/level_manager.cc b/test/performance/level_manager.cc index 1a6100b06c3..172c70db951 100644 --- a/test/performance/level_manager.cc +++ b/test/performance/level_manager.cc @@ -46,6 +46,17 @@ TEST(LevelManagerPerfrormance, LevelVsNoLevel) const std::size_t iters = 5000; + + // Reduce potential startup costs by running the server once before + // measuring time differences between levels and no levels. + { + serverConfig.SetUseLevels(true); + gazebo::Server server(serverConfig); + server.SetUpdatePeriod(1ns); + + server.Run(true, 1, false); + } + // Server with levels { serverConfig.SetUseLevels(true); diff --git a/test/worlds/buoyancy_engine.sdf b/test/worlds/buoyancy_engine.sdf new file mode 100644 index 00000000000..51ce5897bab --- /dev/null +++ b/test/worlds/buoyancy_engine.sdf @@ -0,0 +1,72 @@ + + + + + 0.001 + 1.0 + + + + + 1000 + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 0 0 0 + + + 1000 + + 133.3333 + 133.3333 + 133.3333 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + body + buoyant_box + 0.001 + 0.002 + 0.002 + 0.003 + 0.0003 + + + + + diff --git a/test/worlds/camera_sensor_empty_scene.sdf b/test/worlds/camera_sensor_empty_scene.sdf new file mode 100644 index 00000000000..1f5db3260a4 --- /dev/null +++ b/test/worlds/camera_sensor_empty_scene.sdf @@ -0,0 +1,55 @@ + + + + + .001 + 1.0 + + + + + ogre2 + 1 0 0 1 + + + + true + 0 0 1.0 0 0 0 + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + diff --git a/test/worlds/canonical.sdf b/test/worlds/canonical.sdf index b45d3de65b4..e69b2d41c11 100644 --- a/test/worlds/canonical.sdf +++ b/test/worlds/canonical.sdf @@ -10,48 +10,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 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 - - - - 0 0 0 diff --git a/test/worlds/conveyor.sdf b/test/worlds/conveyor.sdf new file mode 100644 index 00000000000..f4cd000ad8d --- /dev/null +++ b/test/worlds/conveyor.sdf @@ -0,0 +1,284 @@ + + + + + + 0.001 + 10.0 + + + + + + + + + + 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 + + + + + 1 + + 0 0 0 0 0 0 + + 6.06 + + 0.002731 + 0 + 0 + 0.032554 + 1.5e-05 + 0.031391 + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + + 0.7 + 150 + 0 1 0 + + + + + + 0 0 0 0 0 0 + + + 5 0.2 0.1 + + + + + 2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + + -2.5 0 0 -1.570796327 0 0 + + + 0.2 + 0.05 + + + + 1 + 0 + + + + base_link + 2.0 + 0.5 + 0.25 + -0.25 + + + + + 0 0 1 0 0 0 + + + 1.06 + + 0.01 + 0 + 0 + 0.01 + 0 + 0.01 + + + + 0 0 0 0 0 0 + + + 0.1 0.1 0.1 + + + + 1 1 1 1 + + + + + + 0.1 0.1 0.1 + + + 0 0 0 0 0 0 + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + Transform control + + + + + false + 230 + 50 + floating + false + #666666 + + + + + + + + + + + false + 200 + 50 + floating + false + #666666 + + + + + diff --git a/test/worlds/demo_joint_types.sdf b/test/worlds/demo_joint_types.sdf index 12bedec0c49..4125a6c1ce4 100644 --- a/test/worlds/demo_joint_types.sdf +++ b/test/worlds/demo_joint_types.sdf @@ -10,46 +10,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 3D View - false - docked - - - ogre - scene - 0.4 0.4 0.4 - 0.8 0.8 0.8 - 1 -2 1.2 0 0.5 2 - - - - - - World control - false - false - 72 - 121 - 1 - - floating - - - - - - - true - true - true - - - true diff --git a/test/worlds/force_torque.sdf b/test/worlds/force_torque.sdf new file mode 100644 index 00000000000..bc394430b28 --- /dev/null +++ b/test/worlds/force_torque.sdf @@ -0,0 +1,436 @@ + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 0 0 0.02 0 0 0 + + + 2.0 + + 0.166933 + 0.166933 + 0.333333 + + + + + + 1 1 0.04 + + + + + 0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + 0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + + 0 0 0.03 0 0 0 + + 0.2 + + 0.0166933 + 0.0166933 + 0.0333333 + + + + + + 0.9 0.9 0.01 + + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + 0.9 0.9 0.01 + + + + + + + base + sensor_plate + + 100 + true + true + force_torque1 + + sensor + parent_to_child + + + + + + + 0 0 0.5 0 0 0 + + + 10 + + + + + 0.25 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.25 + + + + + + + + + 0 5 0 0 0 0 + + 0 0 0.02 0 0 0 + + + 2.0 + + 0.166933 + 0.166933 + 0.333333 + + + + + + 1 1 0.04 + + + + + 0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + 0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + + 0 0 0.03 0 0 0 + + 0.2 + + 0.0166933 + 0.0166933 + 0.0333333 + + + + + + 0.9 0.9 0.01 + + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + 0.9 0.9 0.01 + + + + + + + base + sensor_plate + + 0.1 0 0 0 0 0 + 100 + true + true + force_torque2 + + sensor + parent_to_child + + + + + + + 0 0 0.5 0 0 0 + + + 10 + + + + + 0.25 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.25 + + + + + + + + + 0 10 0 0 0 0 + + 0 0 0.02 0 0 0 + + + 2.0 + + 0.166933 + 0.166933 + 0.333333 + + + + + + 1 1 0.04 + + + + + 0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + 0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + + 0 0 0.03 0 0 0 + + 0.2 + + 0.0166933 + 0.0166933 + 0.0333333 + + + + + + 0.9 0.9 0.01 + + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + 0.9 0.9 0.01 + + + + + + + base + sensor_plate + + 0 0 0.0 30 0 0 + 100 + true + true + force_torque3 + + child + parent_to_child + + + + + + + 0 0 0.5 0 0 0 + + + 10 + + + + + 0.25 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.25 + + + + + + + + diff --git a/test/worlds/graded_buoyancy.sdf b/test/worlds/graded_buoyancy.sdf new file mode 100644 index 00000000000..8ec0ffc2ed8 --- /dev/null +++ b/test/worlds/graded_buoyancy.sdf @@ -0,0 +1,202 @@ + + + + + + + 0.001 + 1.0 + + + + + + + + + + 1000 + + 0 + 1 + + + + + lighter_than_water::ball::body + + + balloon_lighter_than_air + + + neutral_buoyancy::box + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0 0 0 0 + + + + 0 0 0 0 0 0 + + 25 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + + 0 -5 -3 0 0 0 + + 0 0 0 0 0 0 + + 0.1 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + + + 0 5 -3 0 0 0 + + 0 0 0 0 0 0 + + + 1000 + 0 0 0.1 0 0 0 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + + 4 -6 -3 0 0 0 + + + 1000 + 0 0 0.1 0 0 0 + + 86.28907821859966 + 0 + 0 + 86.28907821859966 + 0 + 5.026548245743671 + + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + diff --git a/test/worlds/imu_no_orientation.sdf b/test/worlds/imu_no_orientation.sdf new file mode 100644 index 00000000000..22e17f36636 --- /dev/null +++ b/test/worlds/imu_no_orientation.sdf @@ -0,0 +1,83 @@ + + + + 0 0 -5 + + 0.001 + 1.0 + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + false + + + + + + + diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf index 0f3678be24e..ecb5f96968d 100644 --- a/test/worlds/joint_controller.sdf +++ b/test/worlds/joint_controller.sdf @@ -9,48 +9,6 @@ filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - - 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 diff --git a/test/worlds/joint_position_controller.sdf b/test/worlds/joint_position_controller.sdf index 906521db606..d38f710aac1 100644 --- a/test/worlds/joint_position_controller.sdf +++ b/test/worlds/joint_position_controller.sdf @@ -10,47 +10,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 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 diff --git a/test/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf new file mode 100644 index 00000000000..f730f6069e3 --- /dev/null +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -0,0 +1,118 @@ + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + j1 + true + 1000 + + + + diff --git a/test/worlds/joint_trajectory_controller.sdf b/test/worlds/joint_trajectory_controller.sdf index 4aaee68ca52..69f9ca0d97f 100644 --- a/test/worlds/joint_trajectory_controller.sdf +++ b/test/worlds/joint_trajectory_controller.sdf @@ -17,64 +17,6 @@ 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 - - - @@ -402,7 +344,7 @@ test_custom_topic/velocity_control - 0.6 175 diff --git a/test/worlds/joint_transmitted_wrench.sdf b/test/worlds/joint_transmitted_wrench.sdf new file mode 100644 index 00000000000..48e43b2dbe3 --- /dev/null +++ b/test/worlds/joint_transmitted_wrench.sdf @@ -0,0 +1,162 @@ + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0.02 0 0 0 + + + 2.0 + + 0.166933 + 0.166933 + 0.333333 + + + + + + 1 1 0.04 + + + + + 0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + 0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + + + + + + 0 0 0.03 0 0 0 + + 0.2 + + 0.0166933 + 0.0166933 + 0.0333333 + + + + + + 0.9 0.9 0.01 + + + + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + 0.8 0.2 0.2 1 + + + + + + 0.9 0.9 0.01 + + + + + + + base + sensor_plate + + + + + 0 0 0.5 0 0 0 + + + 10 + + + + + 0.25 + + + + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + 0.0 0.8 0.8 1 + + + + + + 0.25 + + + + + + + + diff --git a/test/worlds/magnetometer.sdf b/test/worlds/magnetometer.sdf index 9fb5bda4caf..9dab71779f1 100644 --- a/test/worlds/magnetometer.sdf +++ b/test/worlds/magnetometer.sdf @@ -14,6 +14,11 @@ filename="ignition-gazebo-magnetometer-system" name="ignition::gazebo::systems::Magnetometer"> + + + true diff --git a/test/worlds/models/include_nested/model.sdf b/test/worlds/models/include_nested/model.sdf index 880423727ad..c60e64947ad 100644 --- a/test/worlds/models/include_nested/model.sdf +++ b/test/worlds/models/include_nested/model.sdf @@ -41,27 +41,4 @@ 0 0 0 0 0 0 - - - - - 0 0 0 0 0 0 - - - - 1 1 1 - - - - - - - 1 1 1 - - - - - diff --git a/test/worlds/nondefault_canonical.sdf b/test/worlds/nondefault_canonical.sdf index 56c303542df..1d0e485846e 100644 --- a/test/worlds/nondefault_canonical.sdf +++ b/test/worlds/nondefault_canonical.sdf @@ -10,50 +10,6 @@ name="ignition::gazebo::systems::SceneBroadcaster"> - - - - - - 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 - /world/default/control - /world/default/stats - - - - true diff --git a/test/worlds/odometry_publisher_3d.sdf b/test/worlds/odometry_publisher_3d.sdf new file mode 100644 index 00000000000..3101526f803 --- /dev/null +++ b/test/worlds/odometry_publisher_3d.sdf @@ -0,0 +1,390 @@ + + + + + + 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.053302 0 0 0 + + + 1.5 + + 0.0347563 + 0 + 0 + 0.07 + 0 + 0.0977 + + + + + + 0.30 0.42 0.11 + + + + + + + 0.15 0.21 0.11 + + + + + + 0.13 -0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + 0 0 0 1.57 0 0 + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_0 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + 0 0 0 1.57 0 0 + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_1 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + 0.13 0.22 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + 0 0 0 1.57 0 0 + + + 0.2 + 0.01 + + + + 0 0 1 1 + + + + + rotor_2 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + -0.13 -0.2 0.023 0 -0 0 + + 0.005 + + 9.75e-07 + 0 + 0 + 4.17041e-05 + 0 + 4.26041e-05 + + + + + + 0.005 + 0.1 + + + + + 0 0 0 1.57 0 0 + + + 0.2 + 0.01 + + + + 1 0 0 1 + + + + + rotor_3 + base_link + + 0 0 1 + + -1e+16 + 1e+16 + + + + + X3 + rotor_0_joint + rotor_0 + ccw + 0.0125 + 0.025 + 8000.0 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 0 + 8.06428e-05 + 1e-06 + motor_speed/0 + 1 + velocity + + + X3 + rotor_1_joint + rotor_1 + ccw + 0.0125 + 0.025 + 8000.0 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 1 + 8.06428e-05 + 1e-06 + motor_speed/1 + 1 + velocity + + + X3 + rotor_2_joint + rotor_2 + cw + 0.0125 + 0.025 + 8000.0 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 2 + 8.06428e-05 + 1e-06 + motor_speed/2 + 1 + velocity + + + X3 + rotor_3_joint + rotor_3 + cw + 0.0125 + 0.025 + 8000.0 + 8.54858e-06 + 0.016 + gazebo/command/motor_speed + 3 + 8.06428e-05 + 1e-06 + motor_speed/3 + 1 + velocity + + + X3 + gazebo/command/twist + enable + base_link + 2.7 2.7 2.7 + 2 3 0.15 + 0.4 0.52 0.18 + 2 2 2 + + + + rotor_0_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_1_joint + 8.54858e-06 + 0.016 + 1 + + + rotor_2_joint + 8.54858e-06 + 0.016 + -1 + + + rotor_3_joint + 8.54858e-06 + 0.016 + -1 + + + + + 3 + + + + + diff --git a/test/worlds/office.sdf b/test/worlds/office.sdf index baa2c159816..9b4df1b5d02 100644 --- a/test/worlds/office.sdf +++ b/test/worlds/office.sdf @@ -1,3 +1,4 @@ + @@ -13,107 +14,6 @@ 0.8 0.8 0.8 false - - - 1000 - 845 -