diff --git a/CMakeLists.txt b/CMakeLists.txt index a261ce51c8..351aa3b631 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-gazebo6 VERSION 6.1.0) +project(ignition-gazebo6 VERSION 6.2.0) #============================================================================ # Find ignition-cmake @@ -50,7 +50,7 @@ set(CMAKE_POLICY_DEFAULT_CMP0077 NEW) # as protobuf could be find transitively by any dependency set(protobuf_MODULE_COMPATIBLE TRUE) -ign_find_package(sdformat12 REQUIRED VERSION 12.1) +ign_find_package(sdformat12 REQUIRED VERSION 12.3) set(SDF_VER ${sdformat12_VERSION_MAJOR}) #-------------------------------------- @@ -99,7 +99,7 @@ ign_find_package (Qt5 #-------------------------------------- # Find ignition-physics -ign_find_package(ignition-physics5 +ign_find_package(ignition-physics5 VERSION 5.1 COMPONENTS heightmap mesh diff --git a/Changelog.md b/Changelog.md index 917a089b79..b4a2011901 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,104 @@ ## 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 @@ -267,6 +366,46 @@ ## Ignition Gazebo 5.x +### 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 @@ -437,7 +576,7 @@ 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) +1. Heightmap physics (with DART) * [Pull request #661](https://github.com/ignitionrobotics/ign-gazebo/pull/661) 1. Adds Mesh Tutorial @@ -1324,6 +1463,40 @@ ## Ignition Gazebo 3.x +### 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. diff --git a/Migration.md b/Migration.md index 7375f9b33e..4fbe67cda4 100644 --- a/Migration.md +++ b/Migration.md @@ -5,6 +5,11 @@ 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 @@ -68,6 +73,11 @@ since pose information is being logged in the `changed_state` topic. * 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/examples/worlds/auv_controls.sdf b/examples/worlds/auv_controls.sdf index e82596c152..224ed03ef5 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/conveyor.sdf b/examples/worlds/conveyor.sdf new file mode 100644 index 0000000000..4769da875b --- /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/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 57e5bed01f..effbeeafff 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/sensors.sdf b/examples/worlds/sensors.sdf index 8574650335..16e1026ae2 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"> + + @@ -210,5 +214,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/tracked_vehicle_simple.sdf b/examples/worlds/tracked_vehicle_simple.sdf new file mode 100644 index 0000000000..cf9cd29acc --- /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/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 9b6f83b92a..67e405659f 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -260,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/components/Collision.hh b/include/ignition/gazebo/components/Collision.hh index 9c57c90c4c..f35f0e95d9 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/JointEffortLimitsCmd.hh b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh new file mode 100644 index 0000000000..b8b9217974 --- /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 0000000000..775937fbff --- /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/JointVelocityLimitsCmd.hh b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh new file mode 100644 index 0000000000..e85905095f --- /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/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index fd2b6a5303..7ef0dd6639 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -226,6 +226,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/VisualCmd.hh b/include/ignition/gazebo/components/VisualCmd.hh new file mode 100644 index 0000000000..794057aabb --- /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/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 353a85e805..5cb571e883 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -26,6 +26,8 @@ #include #include #include +#include +#include "ignition/gazebo/gui/Export.hh" #include "ignition/gazebo/Entity.hh" #include "ignition/gazebo/config.hh" @@ -41,7 +43,7 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { namespace events { /// \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 @@ -79,7 +81,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 @@ -103,68 +105,60 @@ namespace events private: bool fromUser{false}; }; - /// \brief Event that contains newly created and removed entities - class AddedRemovedEntities : 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 { /// \brief Constructor /// \param[in] _newEntities Set of newly created entities /// \param[in] _removedEntities Set of recently removed entities - public: AddedRemovedEntities(const std::set &_newEntities, - const std::set &_removedEntities) - : QEvent(kType), newEntities(_newEntities), - removedEntities(_removedEntities) - { - } + public: GuiNewRemovedEntities(const std::set &_newEntities, + const std::set &_removedEntities); /// \brief Get the set of newly created entities - public: const std::set &NewEntities() const - { - return this->newEntities; - } + public: const std::set &NewEntities() const; /// \brief Get the set of recently removed entities - public: const std::set &RemovedEntities() const - { - return this->removedEntities; - } + public: const std::set &RemovedEntities() const; /// \brief Unique type for this event. static const QEvent::Type kType = QEvent::Type(QEvent::User + 3); - /// \brief Set of newly created entities - private: std::set newEntities; - - /// \brief Set of recently removed entities - private: std::set removedEntities; + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; - /// \brief Event that notifies when new entities have been removed. - class RemovedEntities : 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] _entities All the removed entities - public: explicit RemovedEntities(const std::vector &_entities) - : QEvent(kType), entities(_entities) - { - } + /// \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 data sent with the event. - /// \return The entities being removed. - public: std::vector Data() const - { - return this->entities; - } + /// \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 The removed entities. - private: std::vector entities; + /// \internal + /// \brief Private data pointer + IGN_UTILS_IMPL_PTR(dataPtr) }; /// \brief True if a transform control is currently active (translate / /// rotate / scale). False if we're in selection mode. - class TransformControlModeActive : public QEvent + class IGNITION_GAZEBO_GUI_VISIBLE TransformControlModeActive : public QEvent { /// \brief Constructor /// \param[in] _tranformModeActive is the transform control mode active diff --git a/include/ignition/gazebo/physics/Events.hh b/include/ignition/gazebo/physics/Events.hh new file mode 100644 index 0000000000..668e19a2ad --- /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 49a4a5557e..ef10b2d93e 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -113,11 +113,13 @@ inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { /// \param[in] _scene Pointer to the scene. public: void SetScene(const rendering::ScenePtr &_scene); - /// \brief Set background color of render window + /// \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); diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 5b0f482811..021368dd4f 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -1719,6 +1719,8 @@ void EntityComponentManager::SetState( // Get Component auto comp = this->ComponentImplementation(entity, type); + std::istringstream istr(compMsg.component()); + // Create if new if (nullptr == comp) { @@ -1729,14 +1731,12 @@ void EntityComponentManager::SetState( << compMsg.type() << "]" << std::endl; continue; } - std::istringstream istr(compMsg.component()); newComp->Deserialize(istr); this->CreateComponentImplementation(entity, type, newComp.get()); } // Update component value else { - std::istringstream istr(compMsg.component()); comp->Deserialize(istr); this->dataPtr->AddModifiedComponent(entity); } diff --git a/src/LevelManager.cc b/src/LevelManager.cc index 6aa156dacc..81fa242484 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -201,7 +201,7 @@ void LevelManager::ReadLevelPerformerInfo() 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/Primitives.cc b/src/Primitives.cc index e83880008a..9e34eac618 100644 --- a/src/Primitives.cc +++ b/src/Primitives.cc @@ -53,9 +53,9 @@ constexpr const char * kBoxSdf = R"( - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 @@ -94,9 +94,9 @@ constexpr const char * kSphereSdf = R"( - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 @@ -137,9 +137,9 @@ constexpr const char * kCylinderSdf = R"( - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 @@ -180,9 +180,9 @@ constexpr const char * kCapsuleSdf = R"( - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 @@ -221,9 +221,9 @@ constexpr const char *kEllipsoidSdf = R"( - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.7 0.7 0.7 1 + 1 1 1 1 diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index e2ca63bfc1..9f8b9286c2 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -26,12 +26,37 @@ #include #include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/components/AirPressureSensor.hh" +#include "ignition/gazebo/components/Altimeter.hh" +#include "ignition/gazebo/components/Camera.hh" +#include "ignition/gazebo/components/ChildLinkName.hh" +#include "ignition/gazebo/components/ContactSensor.hh" +#include "ignition/gazebo/components/DepthCamera.hh" +#include "ignition/gazebo/components/ForceTorque.hh" +#include "ignition/gazebo/components/GpuLidar.hh" +#include "ignition/gazebo/components/Imu.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/Light.hh" +#include "ignition/gazebo/components/Link.hh" +#include "ignition/gazebo/components/LogicalCamera.hh" +#include "ignition/gazebo/components/Magnetometer.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/RgbdCamera.hh" +#include "ignition/gazebo/components/SelfCollide.hh" +#include "ignition/gazebo/components/Sensor.hh" #include "ignition/gazebo/components/SourceFilePath.hh" +#include "ignition/gazebo/components/SegmentationCamera.hh" +#include "ignition/gazebo/components/Static.hh" +#include "ignition/gazebo/components/ThermalCamera.hh" +#include "ignition/gazebo/components/ThreadPitch.hh" +#include "ignition/gazebo/components/WindMode.hh" #include "ignition/gazebo/components/World.hh" @@ -277,7 +302,7 @@ namespace sdf_generator // First remove child entities of whose names can be changed during // simulation (eg. models). Then we add them back from the data in the // ECM. - // TODO(addisu) Remove actors and lights + // TODO(addisu) Remove actors std::vector toRemove; if (_elem->HasElement("model")) { @@ -287,6 +312,15 @@ namespace sdf_generator toRemove.push_back(modelElem); } } + if (_elem->HasElement("light")) + { + for (auto lightElem = _elem->GetElement("light"); lightElem; + lightElem = lightElem->GetNextElement("light")) + { + toRemove.push_back(lightElem); + } + } + for (const auto &e : toRemove) { _elem->RemoveChild(e); @@ -294,6 +328,7 @@ namespace sdf_generator auto worldDir = common::parentPath(worldSdf->Data().Element()->FilePath()); + // models _ecm.Each( [&](const Entity &_modelEntity, const components::Model *, const components::ModelSdf *_modelSdf) @@ -386,6 +421,21 @@ namespace sdf_generator return true; }); + // lights + _ecm.Each( + [&](const Entity &_lightEntity, + const components::Light *, + const components::ParentEntity *_parent) -> bool + { + if (_parent->Data() != _entity) + return true; + + auto lightElem = _elem->AddElement("light"); + updateLightElement(lightElem, _ecm, _lightEntity); + + return true; + }); + return true; } @@ -397,10 +447,12 @@ namespace sdf_generator if (!copySdf(_ecm.Component(_entity), _elem)) return false; - // Update sdf based current components. Here are the list of components to - // be updated: + // Update sdf based on current components. Here are the list of components + // to be updated: // - Name // - Pose + // - Static + // - SelfCollide // This list is to be updated as other components become updateable during // simulation auto *nameComp = _ecm.Component(_entity); @@ -421,19 +473,463 @@ namespace sdf_generator } poseElem->Set(poseComp->Data()); + // static + auto *staticComp = _ecm.Component(_entity); + if (staticComp) + _elem->GetElement("static")->Set(staticComp->Data()); + + // self collide + auto *selfCollideComp = _ecm.Component(_entity); + if (selfCollideComp) + _elem->GetElement("self_collide")->Set(selfCollideComp->Data()); + const auto *pathComp = _ecm.Component(_entity); - if (_elem->HasElement("link") && nullptr != pathComp) + if (_elem->HasElement("link")) + { + if (nullptr != pathComp) + { + // Update relative URIs to use absolute paths. Relative URIs work fine + // in included models, but they have to be converted to absolute URIs + // when the included model is expanded. + relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + } + + // update links + sdf::ElementPtr linkElem = _elem->GetElement("link"); + while (linkElem) + { + std::string linkName = linkElem->Get("name"); + auto linkEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(linkName)); + if (linkEnt != kNullEntity) + updateLinkElement(linkElem, _ecm, linkEnt); + linkElem = linkElem->GetNextElement("link"); + } + } + + if (_elem->HasElement("joint")) + { + // update joints + sdf::ElementPtr jointElem = _elem->GetElement("joint"); + while (jointElem) + { + std::string jointName = jointElem->Get("name"); + auto jointEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(jointName)); + if (jointEnt != kNullEntity) + updateJointElement(jointElem, _ecm, jointEnt); + jointElem = jointElem->GetNextElement("joint"); + } + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. Here are the list of components + // to be updated: + // - Name + // - Pose + // - Inertial + // - WindMode + // This list is to be updated as other components become updateable during + // simulation + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + + // inertial + auto inertialComp = _ecm.Component(_entity); + if (inertialComp) { - // Update relative URIs to use absolute paths. Relative URIs work fine in - // included models, but they have to be converted to absolute URIs when - // the included model is expanded. - relativeToAbsoluteUri(_elem, common::parentPath(pathComp->Data())); + math::Inertiald inertial = inertialComp->Data(); + sdf::ElementPtr inertialElem = _elem->GetElement("inertial"); + inertialElem->GetElement("pose")->Set(inertial.Pose()); + const math::MassMatrix3d &massMatrix = inertial.MassMatrix(); + inertialElem->GetElement("mass")->Set(massMatrix.Mass()); + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + inertiaElem->GetElement("ixx")->Set(massMatrix.Ixx()); + inertiaElem->GetElement("ixy")->Set(massMatrix.Ixy()); + inertiaElem->GetElement("ixz")->Set(massMatrix.Ixz()); + inertiaElem->GetElement("iyy")->Set(massMatrix.Iyy()); + inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz()); + inertiaElem->GetElement("izz")->Set(massMatrix.Izz()); } + + // wind mode + auto windModeComp = _ecm.Component(_entity); + if (windModeComp) + { + bool windMode = windModeComp->Data(); + _elem->GetElement("enable_wind")->Set(windMode); + } + + // update sensors + if (_elem->HasElement("sensor")) + { + sdf::ElementPtr sensorElem = _elem->GetElement("sensor"); + while (sensorElem) + { + std::string sensorName = sensorElem->Get("name"); + auto sensorEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(sensorName)); + if (sensorEnt != kNullEntity) + updateSensorElement(sensorElem, _ecm, sensorEnt); + sensorElem = sensorElem->GetNextElement("sensor"); + } + } + + // update lights + if (_elem->HasElement("light")) + { + sdf::ElementPtr lightElem = _elem->GetElement("light"); + while (lightElem) + { + std::string lightName = lightElem->Get("name"); + auto lightEnt = _ecm.EntityByComponents( + components::ParentEntity(_entity), components::Name(lightName)); + if (lightEnt != kNullEntity) + updateLightElement(lightElem, _ecm, lightEnt); + lightElem = lightElem->GetNextElement("light"); + } + } + return true; } + ///////////////////////////////////////////////// + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on current components. + // This list is to be updated as other components become updateable during + // simulation + auto updateSensorNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // camera + auto camComp = _ecm.Component(_entity); + if (camComp) + { + const sdf::Sensor &sensor = camComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // depth camera + auto depthCamComp = _ecm.Component(_entity); + if (depthCamComp) + { + const sdf::Sensor &sensor = depthCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // thermal camera + auto thermalCamComp = _ecm.Component(_entity); + if (thermalCamComp) + { + const sdf::Sensor &sensor = thermalCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // logical camera + auto logicalCamComp = _ecm.Component(_entity); + if (logicalCamComp) + { + // components::LogicalCamera holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = logicalCamComp->Data(); + return updateSensorNameAndPose(); + } + // segmentation camera + auto segmentationCamComp = + _ecm.Component(_entity); + if (segmentationCamComp) + { + const sdf::Sensor &sensor = segmentationCamComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + // gpu lidar + auto gpuLidarComp = _ecm.Component(_entity); + if (gpuLidarComp) + { + const sdf::Sensor &sensor = gpuLidarComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // altimeter + auto altimeterComp = _ecm.Component(_entity); + if (altimeterComp) + { + const sdf::Sensor &sensor = altimeterComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // contact + auto contactComp = _ecm.Component(_entity); + if (contactComp) + { + // components::ContactSensor holds an sdf::ElementPtr instead of an + // sdf::Sensor + _elem = contactComp->Data(); + return updateSensorNameAndPose(); + } + // air pressure + auto airPressureComp = + _ecm.Component(_entity); + if (airPressureComp) + { + const sdf::Sensor &sensor = airPressureComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // force torque + auto forceTorqueComp = _ecm.Component(_entity); + if (forceTorqueComp) + { + const sdf::Sensor &sensor = forceTorqueComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // imu + auto imuComp = _ecm.Component(_entity); + if (imuComp) + { + const sdf::Sensor &sensor = imuComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + // magnetometer + auto magnetometerComp = + _ecm.Component(_entity); + if (magnetometerComp) + { + const sdf::Sensor &sensor = magnetometerComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } + + return true; + } + + ///////////////////////////////////////////////// + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the light component + auto updateLightNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // light + auto lightComp = _ecm.Component(_entity); + if (lightComp) + { + const sdf::Light &light = lightComp->Data(); + _elem->Copy(light.ToElement()); + return updateLightNameAndPose(); + } + return true; + } + + ///////////////////////////////////////////////// + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity) + { + // Update sdf based on the joint component + auto updateJointNameAndPose = [&] + { + // override name and pose sdf element using values from ECM + auto *nameComp = _ecm.Component(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_entity); + auto poseElem = _elem->GetElement("pose"); + + // Remove all attributes of poseElem + for (const auto *attrName : {"relative_to", "degrees", "rotation_format"}) + { + sdf::ParamPtr attr = poseElem->GetAttribute(attrName); + if (nullptr != attr) + { + attr->Reset(); + } + } + poseElem->Set(poseComp->Data()); + return true; + }; + + // joint + auto jointComp = _ecm.Component(_entity); + if (!jointComp) + { + return false; + } + + // joint type + auto jointTypeComp = _ecm.Component(_entity); + sdf::JointType jointType = jointTypeComp->Data(); + if (jointTypeComp) + { + std::string jointTypeStr = "invalid"; + switch (jointType) + { + case sdf::JointType::BALL: + jointTypeStr = "ball"; + break; + case sdf::JointType::CONTINUOUS: + jointTypeStr = "continuous"; + break; + case sdf::JointType::FIXED: + jointTypeStr = "fixed"; + break; + case sdf::JointType::PRISMATIC: + jointTypeStr = "prismatic"; + break; + case sdf::JointType::GEARBOX: + jointTypeStr = "gearbox"; + break; + case sdf::JointType::REVOLUTE: + jointTypeStr = "revolute"; + break; + case sdf::JointType::REVOLUTE2: + jointTypeStr = "revolute2"; + break; + case sdf::JointType::SCREW: + jointTypeStr = "screw"; + break; + case sdf::JointType::UNIVERSAL: + jointTypeStr = "universal"; + break; + default: + break; + } + _elem->GetAttribute("type")->Set(jointTypeStr); + } + + // parent + auto parentLinkNameComp = + _ecm.Component(_entity); + if (parentLinkNameComp) + { + _elem->GetElement("parent")->Set(parentLinkNameComp->Data()); + } + // child + auto childLinkNameComp = _ecm.Component(_entity); + if (childLinkNameComp) + { + _elem->GetElement("child")->Set(childLinkNameComp->Data()); + } + // thread pitch + auto threadPitchComp = _ecm.Component(_entity); + if (threadPitchComp && jointType == sdf::JointType::SCREW) + { + _elem->GetElement("thread_pitch")->Set(threadPitchComp->Data()); + } + // axis + auto jointAxisComp = _ecm.Component(_entity); + if (jointAxisComp) + { + const sdf::JointAxis axis = jointAxisComp->Data(); + _elem->GetElement("axis")->Copy(axis.ToElement()); + } + // axis2 + auto jointAxis2Comp = _ecm.Component(_entity); + if (jointAxis2Comp) + { + const sdf::JointAxis axis2 = jointAxis2Comp->Data(); + _elem->GetElement("axis2")->Copy(axis2.ToElement(1u)); + } + + // sensors + // remove existing ones in sdf element and add new ones from ECM. + std::vector toRemove; + if (_elem->HasElement("sensor")) + { + for (auto sensorElem = _elem->GetElement("sensor"); sensorElem; + sensorElem = sensorElem->GetNextElement("sensor")) + { + toRemove.push_back(sensorElem); + } + } + for (const auto &e : toRemove) + { + _elem->RemoveChild(e); + } + + auto sensorEntities = _ecm.EntitiesByComponents( + components::ParentEntity(_entity), components::Sensor()); + + for (const auto &sensorEnt : sensorEntities) + { + sdf::ElementPtr sensorElem = _elem->AddElement("sensor"); + updateSensorElement(sensorElem, _ecm, sensorEnt); + } + + return updateJointNameAndPose(); + } + ///////////////////////////////////////////////// /// \brief Checks if a string is a number /// \param[in] _str The string to check diff --git a/src/SdfGenerator.hh b/src/SdfGenerator.hh index e580fe53e8..83a7844c29 100644 --- a/src/SdfGenerator.hh +++ b/src/SdfGenerator.hh @@ -99,6 +99,49 @@ namespace sdf_generator const EntityComponentManager &_ecm, const Entity &_entity, const std::string &_uri); + /// \brief Update an sdf::Element of a link. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Link entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLinkElement(const sdf::ElementPtr &_elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a sensor. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Sensor entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateSensorElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a light. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity Light entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateLightElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); + + /// \brief Update an sdf::Element of a joint. + /// Intended for internal use. + /// \input[in, out] _elem sdf::Element to update + /// \input[in] _ecm Immutable reference to the Entity Component Manager + /// \input[in] _entity joint entity + /// \returns true if update succeeded. + IGNITION_GAZEBO_VISIBLE + bool updateJointElement(sdf::ElementPtr _elem, + const EntityComponentManager &_ecm, + const Entity &_entity); } // namespace sdf_generator } // namespace IGNITION_GAZEBO_VERSION_NAMESPACE } // namespace gazebo diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index aace52588f..3599fda3ef 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -117,6 +117,43 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, << "'"; } } + else if (valA->GetTypeName() == "double") + { + double dblA, dblB; + valA->Get(dblA); + valB->Get(dblB); + if (!math::equal(dblA, dblB)) + { + return testing::AssertionFailure() + << "Mismatch in value as double: '" << dblA << "' vs '" + << dblB << "'"; + } + } + else if (valA->GetTypeName() == "float") + { + float fltA, fltB; + valA->Get(fltA); + valB->Get(fltB); + if (!math::equal(fltA, fltB)) + { + return testing::AssertionFailure() + << "Mismatch in value as float: '" << fltA << "' vs '" + << fltB << "'"; + } + } + else if (valA->GetTypeName() == "bool") + { + bool boolA, boolB; + valA->Get(boolA); + valB->Get(boolB); + if (boolA != boolB) + { + return testing::AssertionFailure() + << "Mismatch in value as bool: '" << boolA << "' vs '" + << boolB << "'"; + } + + } else if (valA->GetAsString() != valB->GetAsString()) { return testing::AssertionFailure() @@ -148,6 +185,27 @@ static testing::AssertionResult isSubset(const sdf::ElementPtr &_elemA, if (!result) { + // Ignore missing pose values if the pose is zero. + sdf::ParamPtr childValA = childElemA->GetValue(); + if (childValA->GetTypeName() == "pose") + { + math::Pose3d childValPose; + childValA->Get(childValPose); + if (childValPose == math::Pose3d::Zero) + return testing::AssertionSuccess(); + } + else if (childValA->GetTypeName() == "bool") + { + bool childValBool; + childValA->Get(childValBool); + if (!childValBool && (childElemA->GetName() == "static" || + childElemA->GetName() == "self_collide" || + childElemA->GetName() == "enable_wind" )) + { + return testing::AssertionSuccess(); + } + } + return testing::AssertionFailure() << "No matching child element in element B for child element '" << childElemA->GetName() << "' in element A"; @@ -163,15 +221,15 @@ TEST(CompareElements, CompareWithDuplicateElements) const std::string m1Sdf = R"( - 0 0 0 0 0 0 + 1 0 0 0 0 0 )"; const std::string m1CompTestSdf = R"( - 0 0 0 0 0 0 - 0 0 0 0 0 0 + 1 0 0 0 0 0 + 0 1 0 0 0 0 )"; @@ -271,7 +329,7 @@ class ModelElementFixture : public ElementUpdateFixture auto elem = std::make_shared(); sdf::initFile("model.sdf", elem); - updateModelElement(elem, this->ecm, model); + EXPECT_TRUE(updateModelElement(elem, this->ecm, model)); return elem; } diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 90570dc844..fb0d9fdace 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -220,6 +220,7 @@ 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), diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index ee7e06515e..e5e84bfe15 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -1022,7 +1022,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"); @@ -1030,8 +1030,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); diff --git a/src/SimulationRunner_TEST.cc b/src/SimulationRunner_TEST.cc index c77cfaafc7..fbcb8d19af 100644 --- a/src/SimulationRunner_TEST.cc +++ b/src/SimulationRunner_TEST.cc @@ -1569,6 +1569,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/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index fa9d276b0f..ae0de7d3da 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -1,6 +1,7 @@ set (gui_sources AboutDialogHandler.cc Gui.cc + GuiEvents.cc GuiFileHandler.cc GuiRunner.cc PathManager.cc @@ -8,6 +9,7 @@ set (gui_sources set (gtest_sources Gui_TEST.cc + GuiEvents_TEST.cc ) add_subdirectory(plugins) diff --git a/src/gui/GuiEvents.cc b/src/gui/GuiEvents.cc new file mode 100644 index 0000000000..dd89de306f --- /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 0000000000..cb0bebc999 --- /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/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 8334f40508..cb29338b8d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -32,12 +33,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" @@ -47,6 +51,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" @@ -64,7 +69,10 @@ #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" @@ -287,6 +295,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) @@ -314,6 +329,39 @@ 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, @@ -433,6 +481,7 @@ ComponentInspector::ComponentInspector() : GuiSystem(), dataPtr(std::make_unique()) { qRegisterMetaType(); + qRegisterMetaType("Entity"); } ///////////////////////////////////////////////// @@ -628,6 +677,13 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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( @@ -635,6 +691,16 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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( @@ -651,6 +717,12 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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( @@ -802,6 +874,33 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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); @@ -818,6 +917,16 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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( @@ -851,6 +960,15 @@ void ComponentInspector::Update(const UpdateInfo &_info, 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()); + } + } else if (this->dataPtr->componentCreators.find(typeId) != this->dataPtr->componentCreators.end()) { @@ -930,13 +1048,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) @@ -1110,6 +1228,96 @@ 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, diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh index 12fc3caac5..1c23dddf1a 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.hh +++ b/src/gui/plugins/component_inspector/ComponentInspector.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -123,6 +124,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. @@ -172,8 +180,8 @@ namespace gazebo /// \brief Entity Q_PROPERTY( - int entity - READ Entity + Entity entity + READ GetEntity WRITE SetEntity NOTIFY EntityChanged ) @@ -283,6 +291,35 @@ 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 @@ -305,11 +342,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 ea95e6e838..9e25fb8c93 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qml +++ b/src/gui/plugins/component_inspector/ComponentInspector.qml @@ -142,6 +142,22 @@ 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++ */ diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 6146914e7d..d3f1739982 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -5,12 +5,15 @@ Boolean.qml ComponentInspector.qml ExpandingTypeHeader.qml + Float.qml Imu.qml + Integer.qml JointType.qml Lidar.qml LidarScan.qml Light.qml Magnetometer.qml + Material.qml NoData.qml Noise.qml Physics.qml diff --git a/src/gui/plugins/component_inspector/Float.qml b/src/gui/plugins/component_inspector/Float.qml index a72d7801be..f01cfb8e35 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 316c2b2e7c..12c9b5174e 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/Material.qml b/src/gui/plugins/component_inspector/Material.qml new file mode 100644 index 0000000000..a7d82cc6c8 --- /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/entity_tree/EntityTree.cc b/src/gui/plugins/entity_tree/EntityTree.cc index 20aa36916a..38a80a9557 100644 --- a/src/gui/plugins/entity_tree/EntityTree.cc +++ b/src/gui/plugins/entity_tree/EntityTree.cc @@ -555,13 +555,15 @@ bool EntityTree::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::AddedRemovedEntities::kType) + ignition::gazebo::gui::events::GuiNewRemovedEntities::kType) { std::lock_guard lock(this->dataPtr->newRemovedEntityMutex); auto addedRemovedEvent = - reinterpret_cast(_event); + 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); diff --git a/src/gui/plugins/scene_manager/GzSceneManager.cc b/src/gui/plugins/scene_manager/GzSceneManager.cc index baf5e8a1b0..6014d14b9d 100644 --- a/src/gui/plugins/scene_manager/GzSceneManager.cc +++ b/src/gui/plugins/scene_manager/GzSceneManager.cc @@ -18,7 +18,7 @@ #include "GzSceneManager.hh" -#include +#include #include #include @@ -104,16 +104,25 @@ void GzSceneManager::Update(const UpdateInfo &_info, this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - // Emit entities removed event - std::vector removed; + // 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.push_back(_entity); + removed.insert(_entity); return true; }); - ignition::gazebo::gui::events::RemovedEntities removedEvent(removed); + ignition::gazebo::gui::events::NewRemovedEntities removedEvent( + created, removed); ignition::gui::App()->sendEvent( ignition::gui::App()->findChild(), &removedEvent); diff --git a/src/gui/plugins/select_entities/SelectEntities.cc b/src/gui/plugins/select_entities/SelectEntities.cc index c08dc0963b..d3513888f7 100644 --- a/src/gui/plugins/select_entities/SelectEntities.cc +++ b/src/gui/plugins/select_entities/SelectEntities.cc @@ -574,13 +574,13 @@ bool SelectEntities::eventFilter(QObject *_obj, QEvent *_event) } } else if (_event->type() == - ignition::gazebo::gui::events::RemovedEntities::kType) + ignition::gazebo::gui::events::NewRemovedEntities::kType) { if (!this->dataPtr->wireBoxes.empty()) { - auto removedEvent = - reinterpret_cast(_event); - for (auto &entity : removedEvent->Data()) + auto event = + reinterpret_cast(_event); + for (auto &entity : event->RemovedEntities()) { auto wireBoxIt = this->dataPtr->wireBoxes.find(entity); if (wireBoxIt != this->dataPtr->wireBoxes.end()) diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 9f7213f488..bdf494d73c 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -233,8 +233,8 @@ ColumnLayout { Layout.row: 0 Layout.column: 1 value: ViewAngle.camPose[0] - maximumValue: 1000000 - minimumValue: -1000000 + 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) @@ -252,8 +252,8 @@ ColumnLayout { Layout.row: 1 Layout.column: 1 value: ViewAngle.camPose[1] - maximumValue: 1000000 - minimumValue: -1000000 + 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) @@ -271,8 +271,8 @@ ColumnLayout { Layout.row: 2 Layout.column: 1 value: ViewAngle.camPose[2] - maximumValue: 1000000 - minimumValue: -1000000 + 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) @@ -383,7 +383,7 @@ ColumnLayout { Layout.row: 0 Layout.column: 3 value: ViewAngle.camClipDist[1] - maximumValue: 1000000 + maximumValue: Number.MAX_VALUE minimumValue: nearClip.value decimals: 6 stepSize: 0.01 diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 4a70ac7b36..3df544c588 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -85,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" @@ -205,11 +206,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; @@ -294,6 +299,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; @@ -694,6 +718,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); + } + } } ////////////////////////////////////////////////// @@ -979,6 +1038,7 @@ 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); @@ -1007,6 +1067,7 @@ 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(); @@ -1035,8 +1096,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()) @@ -1454,6 +1523,66 @@ void RenderUtil::Update() } this->dataPtr->UpdateThermalCamera(thermalCameraData); + + // 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()) + { + color = msgs::Convert(matMsg.ambient()); + if (geomMat->Ambient() != color) + geomMat->SetAmbient(color); + } + + if (matMsg.has_diffuse()) + { + color = msgs::Convert(matMsg.diffuse()); + if (geomMat->Diffuse() != color) + geomMat->SetDiffuse(color); + } + + if (matMsg.has_specular()) + { + color = msgs::Convert(matMsg.specular()); + if (geomMat->Specular() != color) + geomMat->SetSpecular(color); + } + + if (matMsg.has_emissive()) + { + color = msgs::Convert(matMsg.emissive()); + if (geomMat->Emissive() != color) + geomMat->SetEmissive(color); + } + } + } + } + } } ////////////////////////////////////////////////// @@ -2349,8 +2478,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); } } diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c5e7770470..c00277aadf 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() @@ -129,6 +130,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 c3e45a81ec..10db4bff00 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 65744608a6..ca77a04077 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 ff91c83471..fc544941b3 100644 --- a/src/systems/buoyancy/Buoyancy.cc +++ b/src/systems/buoyancy/Buoyancy.cc @@ -441,11 +441,6 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, { 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); @@ -477,6 +472,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, else if (this->dataPtr->buoyancyType == BuoyancyPrivate::BuoyancyType::GRADED_BUOYANCY) { + if (newPose) + { + // Skip entity if WorldPose and inertial are not yet ready + // TODO(arjo): Find a way of disabling gravity effects for + // this first iteration. + return true; + } std::vector collisions = _ecm.ChildrenByComponents( _entity, components::Collision()); this->dataPtr->buoyancyForces.clear(); @@ -521,12 +523,13 @@ void Buoyancy::PreUpdate(const ignition::gazebo::UpdateInfo &_info, } } } - } - auto [force, torque] = this->dataPtr->ResolveForces( + 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.AddWorldWrench(_ecm, force, torque); + // Apply the wrench to the link. This wrench is applied in the + // Physics System. + link.AddWorldWrench(_ecm, force, torque); + } + return true; }); } diff --git a/src/systems/force_torque/ForceTorque.cc b/src/systems/force_torque/ForceTorque.cc index 56f78a4e21..e1668fbf71 100644 --- a/src/systems/force_torque/ForceTorque.cc +++ b/src/systems/force_torque/ForceTorque.cc @@ -136,8 +136,7 @@ void ForceTorque::PostUpdate(const UpdateInfo &_info, for (auto &it : this->dataPtr->entitySensorMap) { - // Update measurement time - it.second->Update(_info.simTime); + it.second.get()->sensors::Sensor::Update(_info.simTime, false); } } diff --git a/src/systems/joint_state_publisher/JointStatePublisher.cc b/src/systems/joint_state_publisher/JointStatePublisher.cc index 9830edfbd7..f19b1b155e 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 352137edae..75c13f15a7 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/logical_camera/LogicalCamera.cc b/src/systems/logical_camera/LogicalCamera.cc index 9a16fcf40c..f9d2ac6215 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 227558414b..62ce17a608 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 61a6312c0d..3a591d75f5 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 f60e309d06..832d3ac301 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/physics/Physics.cc b/src/systems/physics/Physics.cc index dcaeb8ef30..ec96ed1bf0 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -48,6 +48,7 @@ #include #include +#include #include #include #include @@ -102,11 +103,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" @@ -132,6 +136,9 @@ #include "ignition/gazebo/components/HaltMotion.hh" #include "CanonicalLinkModelTracker.hh" +// Events +#include "ignition/gazebo/physics/Events.hh" + #include "EntityFeatureMap.hh" using namespace ignition; @@ -287,6 +294,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; @@ -317,6 +332,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; @@ -453,6 +471,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>; @@ -490,6 +514,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 : @@ -544,6 +590,7 @@ class ignition::gazebo::systems::PhysicsPrivate MinimumFeatureList, CollisionFeatureList, ContactFeatureList, + SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, SolverFeatureList>; @@ -584,7 +631,10 @@ class ignition::gazebo::systems::PhysicsPrivate JointFeatureList, DetachableJointFeatureList, JointVelocityCommandFeatureList, - JointGetTransmittedWrenchFeatureList + JointGetTransmittedWrenchFeatureList, + JointPositionLimitsCommandFeatureList, + JointVelocityLimitsCommandFeatureList, + JointEffortLimitsCommandFeatureList >; /// \brief A map between joint entity ids in the ECM to Joint Entities in @@ -615,6 +665,15 @@ class ignition::gazebo::systems::PhysicsPrivate /// 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; + /// \brief Set of links that were added to an existing model. This set /// is used to track links that were added to an existing model, such as /// through the GUI model editor, so that we can avoid premature creation @@ -638,7 +697,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; @@ -751,7 +810,10 @@ void Physics::Configure(const Entity &_entity, ignerr << "Failed to load a valid physics engine from [" << pathToLib << "]." << std::endl; + return; } + + this->dataPtr->eventManager = &_eventMgr; } ////////////////////////////////////////////////// @@ -1477,6 +1539,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; + }); } ////////////////////////////////////////////////// @@ -1506,6 +1603,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)) { @@ -1518,6 +1616,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); @@ -1607,6 +1715,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; @@ -1632,6 +1752,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( @@ -2138,7 +2351,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( @@ -2786,6 +3000,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 @@ -2801,6 +3029,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 { @@ -3031,6 +3280,7 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( const LinkPtrType &_link, const math::Pose3d &_pose) const { @@ -3040,6 +3290,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/Sensors.cc b/src/systems/sensors/Sensors.cc index a2739240ca..5d802189b1 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -179,6 +179,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; }; ////////////////////////////////////////////////// @@ -199,6 +205,10 @@ 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); @@ -395,10 +405,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, diff --git a/src/systems/sensors/Sensors.hh b/src/systems/sensors/Sensors.hh index 3f988fa146..e871b9b180 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 eefd5b0e90..6211434bc9 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 176cc82398..ec1a607d21 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 0000000000..a1f7ac1df7 --- /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 0000000000..116d3cb787 --- /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 0000000000..f9d54a3204 --- /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 0000000000..8ee3b43f1a --- /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 0000000000..e175d44329 --- /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 0000000000..878c5defc9 --- /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 f6f0a7b486..6993461c23 100644 --- a/src/systems/user_commands/UserCommands.cc +++ b/src/systems/user_commands/UserCommands.cc @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -59,6 +60,7 @@ #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; @@ -344,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); + }}; +}; } } } @@ -354,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, @@ -362,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( @@ -370,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, @@ -378,21 +438,21 @@ 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); /// \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); @@ -407,7 +467,7 @@ class ignition::gazebo::systems::UserCommandsPrivate /// \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( @@ -415,12 +475,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; @@ -568,6 +635,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; } ////////////////////////////////////////////////// @@ -754,6 +829,24 @@ 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) @@ -1446,6 +1539,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/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index a2fec80165..a27b236ab6 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -53,7 +53,9 @@ set(tests 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 @@ -65,6 +67,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 @@ -107,6 +110,24 @@ ign_build_tests(TYPE INTEGRATION ${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 5ab6ba4ca2..481c7d29ee 100644 --- a/test/integration/buoyancy.cc +++ b/test/integration/buoyancy.cc @@ -57,7 +57,7 @@ TEST_F(BuoyancyTest, UniformWorldMovement) using namespace std::chrono_literals; server.SetUpdatePeriod(1ns); - std::size_t iterations = 1001; + std::size_t iterations = 1000; bool finished = false; test::Relay testSystem; diff --git a/test/integration/camera_sensor_background.cc b/test/integration/camera_sensor_background.cc new file mode 100644 index 0000000000..1b7bbdc5e6 --- /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 e84ba44a42..93a507e83e 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -50,7 +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" @@ -595,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) { diff --git a/test/integration/force_torque_system.cc b/test/integration/force_torque_system.cc index d3fce78236..a6f769e0ed 100644 --- a/test/integration/force_torque_system.cc +++ b/test/integration/force_torque_system.cc @@ -54,18 +54,21 @@ TEST_F(ForceTorqueTest, MeasureWeight) EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(1us); - size_t iters = 1000u; + // 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(iters); + wrenches.reserve(updates); std::mutex wrenchMutex; std::condition_variable cv; auto wrenchCb = std::function( - [&wrenchMutex, &wrenches, &cv, iters](const auto &_msg) + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) { std::lock_guard lock(wrenchMutex); wrenches.push_back(_msg); - if (wrenches.size() >= iters) + if (wrenches.size() >= updates) { cv.notify_all(); } @@ -80,8 +83,8 @@ TEST_F(ForceTorqueTest, MeasureWeight) { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const double kSensorMass = 0.2; const double kWeightMass = 10; @@ -109,18 +112,21 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(1us); - size_t iters = 1000u; + // 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(iters); + wrenches.reserve(updates); std::mutex wrenchMutex; std::condition_variable cv; auto wrenchCb = std::function( - [&wrenchMutex, &wrenches, &cv, iters](const auto &_msg) + [&wrenchMutex, &wrenches, &cv, updates](const auto &_msg) { std::lock_guard lock(wrenchMutex); wrenches.push_back(_msg); - if (wrenches.size() >= iters) + if (wrenches.size() >= updates) { cv.notify_all(); } @@ -138,8 +144,8 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) const double kGravity = 9.8; { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const double kMomentArm = 0.1; const auto &wrench = wrenches.back(); @@ -157,8 +163,8 @@ TEST_F(ForceTorqueTest, SensorPoseOffset) ASSERT_EQ(2 * iters, *server.IterationCount()); { std::unique_lock lock(wrenchMutex); - cv.wait_for(lock, 30s, [&] { return wrenches.size() >= iters; }); - ASSERT_EQ(iters, wrenches.size()); + cv.wait_for(lock, 30s, [&] { return wrenches.size() >= updates; }); + ASSERT_EQ(updates, wrenches.size()); const auto &wrench = wrenches.back(); diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index 00c8ab4fc0..e918d2885e 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/physics_system.cc b/test/integration/physics_system.cc index 38f4fdd07c..cb21e18454 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -21,6 +21,9 @@ #include #include +#ifdef HAVE_DART +#include +#endif #include #include #include @@ -45,10 +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" @@ -74,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) { @@ -812,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) { diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 7f9f7072e2..0e3964b052 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -20,10 +20,21 @@ #include #include +#include +#include +#include #include +#include +#include +#include +#include +#include #include +#include +#include #include #include +#include #include #include @@ -428,6 +439,523 @@ TEST_F(SdfGeneratorFixture, ModelWithNestedIncludes) EXPECT_TRUE(err.empty()); } +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithSensors) +{ + this->LoadWorld("test/worlds/non_rendering_sensors.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("non_rendering_sensors"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + + // altimeter + { + auto *sensor = link->SensorByName("altimeter_sensor"); + ASSERT_NE(nullptr, sensor); + const sdf::Altimeter *altimeter = sensor->AltimeterSensor(); + ASSERT_NE(nullptr, altimeter); + const sdf::Noise &posNoise = altimeter->VerticalPositionNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, posNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, posNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, posNoise.StdDev()); + + const sdf::Noise &velNoise = altimeter->VerticalVelocityNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, velNoise.Type()); + EXPECT_DOUBLE_EQ(2.3, velNoise.Mean()); + EXPECT_DOUBLE_EQ(4.5, velNoise.StdDev()); + } + + // contact sensor + { + // contact sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("contact_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::CONTACT, sensor->Type()); + } + + // force torque + { + auto *sensor = link->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::ForceTorque *forceTorque = sensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorque); + EXPECT_EQ(sdf::ForceTorqueFrame::CHILD, forceTorque->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorque->MeasureDirection()); + const sdf::Noise &forceXNoise = forceTorque->ForceXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, forceXNoise.Type()); + EXPECT_DOUBLE_EQ(0.02, forceXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0005, forceXNoise.StdDev()); + const sdf::Noise &torqueYNoise = forceTorque->TorqueYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, torqueYNoise.Type()); + EXPECT_DOUBLE_EQ(0.009, torqueYNoise.Mean()); + EXPECT_DOUBLE_EQ(0.0000985, torqueYNoise.StdDev()); + } + + // imu + { + auto *sensor = link->SensorByName("imu_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(4, 5, 6, 0, 0, 0), sensor->RawPose()); + const sdf::Imu *imu = sensor->ImuSensor(); + ASSERT_NE(nullptr, imu); + const sdf::Noise &linAccXNoise = imu->LinearAccelerationXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccXNoise.Type()); + EXPECT_DOUBLE_EQ(0.0, linAccXNoise.Mean()); + EXPECT_DOUBLE_EQ(0.1, linAccXNoise.StdDev()); + EXPECT_DOUBLE_EQ(0.2, linAccXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(1.0, linAccXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccYNoise = imu->LinearAccelerationYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccYNoise.Type()); + EXPECT_DOUBLE_EQ(1.0, linAccYNoise.Mean()); + EXPECT_DOUBLE_EQ(1.1, linAccYNoise.StdDev()); + EXPECT_DOUBLE_EQ(1.2, linAccYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(2.0, linAccYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &linAccZNoise = imu->LinearAccelerationZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, linAccZNoise.Type()); + EXPECT_DOUBLE_EQ(2.0, linAccZNoise.Mean()); + EXPECT_DOUBLE_EQ(2.1, linAccZNoise.StdDev()); + EXPECT_DOUBLE_EQ(2.2, linAccZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(3.0, linAccZNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelXNoise = imu->AngularVelocityXNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelXNoise.Type()); + EXPECT_DOUBLE_EQ(3.0, angVelXNoise.Mean()); + EXPECT_DOUBLE_EQ(3.1, angVelXNoise.StdDev()); + EXPECT_DOUBLE_EQ(4.2, angVelXNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(4.0, angVelXNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelYNoise = imu->AngularVelocityYNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelYNoise.Type()); + EXPECT_DOUBLE_EQ(4.0, angVelYNoise.Mean()); + EXPECT_DOUBLE_EQ(4.1, angVelYNoise.StdDev()); + EXPECT_DOUBLE_EQ(5.2, angVelYNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(5.0, angVelYNoise.DynamicBiasCorrelationTime()); + const sdf::Noise &angVelZNoise = imu->AngularVelocityZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, angVelZNoise.Type()); + EXPECT_DOUBLE_EQ(5.0, angVelZNoise.Mean()); + EXPECT_DOUBLE_EQ(5.1, angVelZNoise.StdDev()); + EXPECT_DOUBLE_EQ(6.2, angVelZNoise.DynamicBiasStdDev()); + EXPECT_DOUBLE_EQ(6.0, angVelZNoise.DynamicBiasCorrelationTime()); + + EXPECT_EQ("ENU", imu->Localization()); + EXPECT_EQ("linka", imu->CustomRpyParentFrame()); + EXPECT_EQ(math::Vector3d::UnitY, imu->CustomRpy()); + EXPECT_EQ("linkb", imu->GravityDirXParentFrame()); + EXPECT_EQ(math::Vector3d::UnitZ, imu->GravityDirX()); + EXPECT_FALSE(imu->OrientationEnabled()); + } + + // logical camera + { + // logical camera sensor does not have an SDF DOM class + auto *sensor = link->SensorByName("logical_camera_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(7, 8, 9, 0, 0, 0), sensor->RawPose()); + EXPECT_EQ(sdf::SensorType::LOGICAL_CAMERA, sensor->Type()); + } + + // magnetometer + { + auto *sensor = link->SensorByName("magnetometer_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 11, 12, 0, 0, 0), sensor->RawPose()); + const sdf::Magnetometer *magnetometer = sensor->MagnetometerSensor(); + ASSERT_NE(nullptr, magnetometer); + const sdf::Noise &xNoise = magnetometer->XNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, xNoise.Type()); + EXPECT_DOUBLE_EQ(0.1, xNoise.Mean()); + EXPECT_DOUBLE_EQ(0.2, xNoise.StdDev()); + const sdf::Noise &yNoise = magnetometer->YNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, yNoise.Type()); + EXPECT_DOUBLE_EQ(1.2, yNoise.Mean()); + EXPECT_DOUBLE_EQ(2.3, yNoise.StdDev()); + const sdf::Noise &zNoise = magnetometer->ZNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, zNoise.Type()); + EXPECT_DOUBLE_EQ(3.4, zNoise.Mean()); + EXPECT_DOUBLE_EQ(5.6, zNoise.StdDev()); + } + + // air pressure + { + auto *sensor = link->SensorByName("air_pressure_sensor"); + ASSERT_NE(nullptr, sensor); + EXPECT_EQ(math::Pose3d(10, 20, 30, 0, 0, 0), sensor->RawPose()); + const sdf::AirPressure *airPressure = sensor->AirPressureSensor(); + ASSERT_NE(nullptr, airPressure); + EXPECT_DOUBLE_EQ(123.4, airPressure->ReferenceAltitude()); + const sdf::Noise &noise = airPressure->PressureNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN, noise.Type()); + EXPECT_DOUBLE_EQ(3.4, noise.Mean()); + EXPECT_DOUBLE_EQ(5.6, noise.StdDev()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithRenderingSensors) +{ + this->LoadWorld("test/worlds/sensor.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("camera_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // camera + { + EXPECT_TRUE(world->ModelNameExists("camera")); + auto *model = world->ModelByName("camera"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + auto *link = model->LinkByName("link"); + ASSERT_NE(nullptr, link); + math::MassMatrix3d massMatrix(0.1, + math::Vector3d( 0.000166667, 0.000166667, 0.000166667), + math::Vector3d::Zero); + math::Inertiald inertial(massMatrix, math::Pose3d::Zero); + EXPECT_EQ(inertial, link->Inertial()); + + auto *cameraSensor = link->SensorByName("camera"); + ASSERT_NE(nullptr, cameraSensor); + EXPECT_EQ("camera", cameraSensor->Topic()); + const sdf::Camera *camera = cameraSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.047, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(100, camera->FarClip()); + const sdf::Noise &noise = camera->ImageNoise(); + EXPECT_EQ(sdf::NoiseType::GAUSSIAN_QUANTIZED, noise.Type()); + EXPECT_DOUBLE_EQ(0.01, noise.Mean()); + EXPECT_DOUBLE_EQ(0.0002, noise.StdDev()); + } + + EXPECT_TRUE(world->ModelNameExists("default_topics")); + auto *model = world->ModelByName("default_topics"); + ASSERT_NE(nullptr, model); + // gpu lidar + { + auto *gpuLidarLink = model->LinkByName("gpu_lidar_link"); + ASSERT_NE(nullptr, gpuLidarLink); + auto *gpuLidarSensor = gpuLidarLink->SensorByName("gpu_lidar"); + const sdf::Lidar *lidar = gpuLidarSensor->LidarSensor(); + EXPECT_EQ(640u, lidar->HorizontalScanSamples()); + EXPECT_DOUBLE_EQ(1.0, lidar->HorizontalScanResolution()); + EXPECT_NEAR(-1.396263, lidar->HorizontalScanMinAngle().Radian(), 1e-5); + EXPECT_NEAR(1.396263, lidar->HorizontalScanMaxAngle().Radian(), 1e-5); + EXPECT_EQ(1u, lidar->VerticalScanSamples()); + EXPECT_DOUBLE_EQ(0.01, lidar->VerticalScanResolution()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMinAngle().Radian()); + EXPECT_DOUBLE_EQ(0.0, lidar->VerticalScanMaxAngle().Radian()); + EXPECT_DOUBLE_EQ(0.08, lidar->RangeMin()); + EXPECT_DOUBLE_EQ(10.0, lidar->RangeMax()); + EXPECT_DOUBLE_EQ(0.01, lidar->RangeResolution()); + } + + // depth camera + { + auto *depthLink = model->LinkByName("depth_camera_link"); + ASSERT_NE(nullptr, depthLink); + auto *depthSensor = depthLink->SensorByName("depth_camera"); + ASSERT_NE(nullptr, depthSensor); + const sdf::Camera *camera = depthSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_EQ("R_FLOAT32", camera->PixelFormatStr()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + EXPECT_DOUBLE_EQ(0.05, camera->DepthNearClip()); + EXPECT_DOUBLE_EQ(9.0, camera->DepthFarClip()); + } + + // rgbd camera + { + auto *rgbdLink = model->LinkByName("rgbd_camera_link"); + ASSERT_NE(nullptr, rgbdLink); + auto *rgbdSensor = rgbdLink->SensorByName("rgbd_camera"); + ASSERT_NE(nullptr, rgbdSensor); + const sdf::Camera *camera = rgbdSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.05, camera->HorizontalFov().Radian()); + EXPECT_EQ(256u, camera->ImageWidth()); + EXPECT_EQ(256u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.1, camera->NearClip()); + EXPECT_DOUBLE_EQ(10, camera->FarClip()); + } + + // thermal camera + { + auto *thermalLink = model->LinkByName("thermal_camera_link"); + ASSERT_NE(nullptr, thermalLink); + auto *thermalSensor = thermalLink->SensorByName("thermal_camera"); + ASSERT_NE(nullptr, thermalSensor); + const sdf::Camera *camera = thermalSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.15, camera->HorizontalFov().Radian()); + EXPECT_EQ(300u, camera->ImageWidth()); + EXPECT_EQ(200u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.14, camera->NearClip()); + EXPECT_DOUBLE_EQ(120.0, camera->FarClip()); + } + + // segmentation camera + { + auto *segmentationLink = model->LinkByName("segmentation_camera_link"); + ASSERT_NE(nullptr, segmentationLink); + auto *segmentationSensor = + segmentationLink->SensorByName("segmentation_camera"); + ASSERT_NE(nullptr, segmentationSensor); + const sdf::Camera *camera = segmentationSensor->CameraSensor(); + ASSERT_NE(nullptr, camera); + EXPECT_DOUBLE_EQ(1.0, camera->HorizontalFov().Radian()); + EXPECT_EQ(320u, camera->ImageWidth()); + EXPECT_EQ(240u, camera->ImageHeight()); + EXPECT_DOUBLE_EQ(0.11, camera->NearClip()); + EXPECT_DOUBLE_EQ(20.0, camera->FarClip()); + EXPECT_EQ("panoptic", camera->SegmentationType()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, WorldWithLights) +{ + this->LoadWorld("test/worlds/lights.sdf"); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("lights"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + + // directional light in the world + { + const sdf::Light *light = world->LightByIndex(0u); + EXPECT_EQ("directional", light->Name()); + EXPECT_EQ(sdf::LightType::DIRECTIONAL, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 0, 10, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_TRUE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.8f, 0.8f, 0.8f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(100, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.9, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.5, 0.2, -0.9), + light->Direction()); + } + // point light in the world + { + const sdf::Light *light = world->LightByIndex(1u); + EXPECT_EQ("point", light->Name()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, -1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(1.0f, 0.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(4, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.2, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.5, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + // spot light in the world + { + const sdf::Light *light = world->LightByIndex(2u); + EXPECT_EQ("spot", light->Name()); + EXPECT_EQ(sdf::LightType::SPOT, light->Type()); + EXPECT_EQ(ignition::math::Pose3d(0, 1.5, 3, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(std::string(), light->PoseRelativeTo()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 1.0f, 0.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.2f, 0.2f, 0.2f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(5, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.3, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.4, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.001, light->QuadraticAttenuationFactor()); + EXPECT_EQ(ignition::math::Vector3d(0.0, 0.0, -1.0), + light->Direction()); + EXPECT_DOUBLE_EQ(0.1, light->SpotInnerAngle().Radian()); + EXPECT_DOUBLE_EQ(0.5, light->SpotOuterAngle().Radian()); + EXPECT_DOUBLE_EQ(0.8, light->SpotFalloff()); + } + + // get model + EXPECT_TRUE(world->ModelNameExists("sphere")); + auto *model = world->ModelByName("sphere"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(1u, model->LinkCount()); + + // get link + auto *link = model->LinkByName("sphere_link"); + ASSERT_NE(nullptr, link); + + // light attached to link + { + const sdf::Light *light = link->LightByName("link_light_point"); + EXPECT_EQ("link_light_point", light->Name()); + EXPECT_EQ(ignition::math::Pose3d(0.0, 0.0, 1.0, 0, 0, 0), + light->RawPose()); + EXPECT_EQ(sdf::LightType::POINT, light->Type()); + EXPECT_FALSE(light->CastShadows()); + EXPECT_EQ(ignition::math::Color(0.0f, 0.0f, 1.0f, 1), + light->Diffuse()); + EXPECT_EQ(ignition::math::Color(0.1f, 0.1f, 0.1f, 1), + light->Specular()); + EXPECT_DOUBLE_EQ(2, light->AttenuationRange()); + EXPECT_DOUBLE_EQ(0.05, light->ConstantAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.02, light->LinearAttenuationFactor()); + EXPECT_DOUBLE_EQ(0.01, light->QuadraticAttenuationFactor()); + } + + tinyxml2::XMLDocument genSdfDoc; + genSdfDoc.Parse(worldGenSdfRes.c_str()); + ASSERT_NE(nullptr, genSdfDoc.RootElement()); + auto genWorld = genSdfDoc.RootElement()->FirstChildElement("world"); + ASSERT_NE(nullptr, genWorld); +} + +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, ModelWithJoints) +{ + this->LoadWorld(ignition::common::joinPaths("test", "worlds", + "joint_sensor.sdf")); + + const std::string worldGenSdfRes = + this->RequestGeneratedSdf("joint_sensor"); + + sdf::Root root; + sdf::Errors err = root.LoadSdfString(worldGenSdfRes); + EXPECT_TRUE(err.empty()); + auto *world = root.WorldByIndex(0); + ASSERT_NE(nullptr, world); + EXPECT_EQ(1u, world->ModelCount()); + + EXPECT_TRUE(world->ModelNameExists("model")); + auto *model = world->ModelByName("model"); + ASSERT_NE(nullptr, model); + EXPECT_EQ(2u, model->LinkCount()); + auto *link1 = model->LinkByName("link1"); + ASSERT_NE(nullptr, link1); + auto *link2 = model->LinkByName("link2"); + ASSERT_NE(nullptr, link2); + EXPECT_EQ(1u, model->JointCount()); + auto *joint = model->JointByName("joint"); + ASSERT_NE(nullptr, joint); + + EXPECT_EQ("link1", joint->ParentLinkName()); + EXPECT_EQ("link2", joint->ChildLinkName()); + EXPECT_EQ(sdf::JointType::REVOLUTE2, joint->Type()); + + // Get the first axis + const sdf::JointAxis *axis = joint->Axis(); + ASSERT_NE(nullptr, axis); + ASSERT_NE(nullptr, axis->Element()); + + // Get the second axis + const sdf::JointAxis *axis2 = joint->Axis(1); + ASSERT_NE(nullptr, axis2); + + EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis->Xyz()); + EXPECT_EQ(ignition::math::Vector3d::UnitY, axis2->Xyz()); + + EXPECT_EQ("__model__", axis->XyzExpressedIn()); + EXPECT_TRUE(axis2->XyzExpressedIn().empty()); + + EXPECT_DOUBLE_EQ(-0.5, axis->Lower()); + EXPECT_DOUBLE_EQ(0.5, axis->Upper()); + EXPECT_DOUBLE_EQ(-1.0, axis2->Lower()); + EXPECT_DOUBLE_EQ(1.0, axis2->Upper()); + + EXPECT_DOUBLE_EQ(123.4, axis->Effort()); + EXPECT_DOUBLE_EQ(0.5, axis2->Effort()); + + EXPECT_DOUBLE_EQ(12.0, axis->MaxVelocity()); + EXPECT_DOUBLE_EQ(200.0, axis2->MaxVelocity()); + + EXPECT_DOUBLE_EQ(0.1, axis->Damping()); + EXPECT_DOUBLE_EQ(0.0, axis2->Damping()); + + EXPECT_DOUBLE_EQ(0.2, axis->Friction()); + EXPECT_DOUBLE_EQ(0.0, axis2->Friction()); + + EXPECT_DOUBLE_EQ(1.3, axis->SpringReference()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringReference()); + + EXPECT_DOUBLE_EQ(10.6, axis->SpringStiffness()); + EXPECT_DOUBLE_EQ(0.0, axis2->SpringStiffness()); + + // sensor + const sdf::Sensor *forceTorqueSensor = + joint->SensorByName("force_torque_sensor"); + ASSERT_NE(nullptr, forceTorqueSensor); + EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name()); + EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type()); + EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0), + forceTorqueSensor->RawPose()); + auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor(); + ASSERT_NE(nullptr, forceTorqueSensorObj); + EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame()); + EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD, + forceTorqueSensorObj->MeasureDirection()); + + EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean()); + EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev()); + EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean()); + EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev()); + EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean()); + EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev()); + + EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean()); + EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev()); + EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean()); + EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev()); + EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean()); + EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev()); +} + ///////////////////////////////////////////////// /// Main int main(int _argc, char **_argv) diff --git a/test/integration/thruster.cc b/test/integration/thruster.cc new file mode 100644 index 0000000000..591bad2580 --- /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 0000000000..ea53742d3b --- /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/worlds/camera_sensor_empty_scene.sdf b/test/worlds/camera_sensor_empty_scene.sdf new file mode 100644 index 0000000000..1f5db3260a --- /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/conveyor.sdf b/test/worlds/conveyor.sdf new file mode 100644 index 0000000000..f4cd000ad8 --- /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/force_torque.sdf b/test/worlds/force_torque.sdf index fe9e177245..bc394430b2 100644 --- a/test/worlds/force_torque.sdf +++ b/test/worlds/force_torque.sdf @@ -132,7 +132,7 @@ base sensor_plate - 30 + 100 true true force_torque1 @@ -262,7 +262,7 @@ sensor_plate 0.1 0 0 0 0 0 - 30 + 100 true true force_torque2 @@ -392,7 +392,7 @@ sensor_plate 0 0 0.0 30 0 0 - 30 + 100 true true force_torque3 diff --git a/test/worlds/joint_sensor.sdf b/test/worlds/joint_sensor.sdf new file mode 100644 index 0000000000..f96cdeefe0 --- /dev/null +++ b/test/worlds/joint_sensor.sdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + link2 + link1 + + 0 0 1 + 0.5 + true + + -0.5 + 0.5 + 123.4 + 12 + + + 0.1 + 0.2 + 1.3 + 10.6 + + + + 0 1 0 + 1.5 + false + + -1 + 1 + 0.5 + 200 + + + + + 1 + + 0 + 0.2 + + + + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + 0 + 0.1 + + + + + 1 + 1.1 + + + + + 2 + 2.1 + + + + + + + 3 + 3.1 + + + + + 4 + 4.1 + + + + + 5 + 5.1 + + + + + + + + + + diff --git a/test/worlds/non_rendering_sensors.sdf b/test/worlds/non_rendering_sensors.sdf new file mode 100644 index 0000000000..f5fac8c8aa --- /dev/null +++ b/test/worlds/non_rendering_sensors.sdf @@ -0,0 +1,176 @@ + + + + + ogre2 + + + + + + + 0 0 3 0 0 0 + + + + + 0.1 + 0.2 + + + + + 2.3 + 4.5 + + + + + + + 4 5 6 0 0 0 + true + + + + 10 11 12 0 0 0 + + child + parent_to_child + + + + 0.02 + 0.0005 + + + + + + + 0.009 + 0.0000985 + + + + + + + + 4 5 6 0 0 0 + + + + + 0 + 0.1 + 0.2 + 1 + + + + + 1 + 1.1 + 1.2 + 2 + + + + + 2 + 2.1 + 2.2 + 3 + + + + + + + 3 + 3.1 + 4.2 + 4 + + + + + 4 + 4.1 + 5.2 + 5 + + + + + 5 + 5.1 + 6.2 + 6 + + + + + ENU + 0 1 0 + 0 0 1 + + false + + + + + 7 8 9 0 0 0 + + 0.1 + 100.1 + 1.33 + 1.234 + + + + + 10 11 12 0 0 0 + + + + 0.1 + 0.2 + + + + + 1.2 + 2.3 + + + + + 3.4 + 5.6 + + + + + + + 10 20 30 0 0 0 + + 123.4 + + + 3.4 + 5.6 + + + + + + + + diff --git a/test/worlds/odometry_publisher_3d.sdf b/test/worlds/odometry_publisher_3d.sdf new file mode 100644 index 0000000000..3101526f80 --- /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/revolute_joint_equilibrium.sdf b/test/worlds/revolute_joint_equilibrium.sdf new file mode 100644 index 0000000000..cf967f17b3 --- /dev/null +++ b/test/worlds/revolute_joint_equilibrium.sdf @@ -0,0 +1,193 @@ + + + + + + + + + + + + + + 3D View + false + false + 0 + + + + + + + ogre + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -1 0 1 0 0.5 0 + + + + + + World control + false + false + 72 + 121 + 1 + + + + + true + true + true + + + + + true + + + + + 0 0 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 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 -0.2 0 0 0 + + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.4 + 0.02 + + + + 1 1 1 1 + 1 1 1 1 + 1 1 1 1 + + + + + + 0.4 + 0.02 + + + + + + base_link + link1 + + + 0 0 -0.55 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.3 + 0.02 + + + + + + + 0.3 + 0.02 + + + + + + + 0 0 0.15 0 0 0 + link1 + link2 + + 1 0 0 + + + + + diff --git a/test/worlds/save_world.sdf b/test/worlds/save_world.sdf index e6a431d4e6..226ef45cb5 100644 --- a/test/worlds/save_world.sdf +++ b/test/worlds/save_world.sdf @@ -13,6 +13,21 @@ filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands"> + 0 0 -9.8 + 5.5645e-6 22.8758e-6 -42.3884e-6 + + + + 0.4 0.4 0.4 1.0 + .7 .7 .7 1 + true + + + + 0.001 + 1.0 + 1000 + 10 0 0 0 0 0 diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index aebb32104c..1587a32736 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -64,6 +64,11 @@ 0.1 100 + + gaussian_quantized + 0.01 + 0.0002 + 1 30 @@ -131,6 +136,12 @@ 0.1 10.0 + + + 0.05 + 9.0 + + @@ -150,6 +161,40 @@ + + + + + 1.15 + + 300 + 200 + + + 0.14 + 120.0 + + + + + + + + + 1.0 + + 320 + 240 + + + 0.11 + 20.0 + + panoptic + + + + diff --git a/test/worlds/thruster_pid.sdf b/test/worlds/thruster_pid.sdf new file mode 100644 index 0000000000..c18becc7d0 --- /dev/null +++ b/test/worlds/thruster_pid.sdf @@ -0,0 +1,111 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.2 0.05 + + + + + + + + 0 0 0 0 3.14159265 0 + body + propeller + + + -1 0 0 + + -1e+12 + 1e+12 + -1 + -1 + + + + + + propeller_joint + 0.004 + 1000 + 0.2 + + + + + diff --git a/test/worlds/thruster_vel_cmd.sdf b/test/worlds/thruster_vel_cmd.sdf new file mode 100644 index 0000000000..f78236a8ca --- /dev/null +++ b/test/worlds/thruster_vel_cmd.sdf @@ -0,0 +1,110 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + custom + propeller_joint + 0.005 + 950 + 0.25 + true + + + + + diff --git a/test/worlds/tracked_vehicle_simple.sdf b/test/worlds/tracked_vehicle_simple.sdf new file mode 100644 index 0000000000..5a54f56958 --- /dev/null +++ b/test/worlds/tracked_vehicle_simple.sdf @@ -0,0 +1,1621 @@ + + + + + 0.001 + 1.0 + 1000 + + + + + + 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 + + + + 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 + + + + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + +