From 7f9f56ae2ba252bae7cfe5bfac38e95b83057531 Mon Sep 17 00:00:00 2001 From: CoolSpy3 <55305038+CoolSpy3@users.noreply.github.com> Date: Sat, 7 Sep 2024 14:28:35 -0700 Subject: [PATCH] Improve Node Field Query API (#6613) * fix typo * send proto ancestry to controllers * expose proto ancestry in controller api * send proto ancestry in all relevant messages * relocate proto data to WbProto * rename WbProto to WbNodeProtoInfo * revert last two commits * keep track of internal proto fields * basic supervisor proto implementation * swap meanings of wb_supervisor_node_get_field and wb_supervisor_node_get_parameter * use an array list rather than a linked list for keeping track of node proto parents * allow supervisor to query protos from Webots * use correct read_only behavior * fix get_proto api * update supervisor/Webots field query api * fix Makefile * fix return reference to temporary * supervisor fixes * more supervisor fixes * fix variable name * fix field get value impl * fix controller field lookup * fix get number of fields/parameters * fix function name * fix wb_supervisor_proto_get_parent * better handling of hidden parameters * remove wb_supervisor_node_get_proto_ancestor * revert 30953fc33133dcfa74dee7b7eb977585c0ab7054...01e067dccae2a7949541d80f213c706fc389faa1 * remove accidental deletion * fix indentation * forward proto field lookups to the actual parameters * fix typo * update supervisor_notify_import_remove test * null safety * add proto tests * fix C_SUPERVISOR_FIELD_GET_FROM_NAME implementation * test fixes * update supervisor_proto test * fix supervisor_proto_fields compilation errors * fix proto import paths * fix bracket type * fix memory leak * remove unused SolidProtoHierarchy.children field (#6630) * add additional log info * bugfix to previous commit * step the simulation after setting field values * fix null return value in wb_supervisor_proto_get_number_of_parameters * fix actual parameter lookup in proto parameter lookup functions * use wb_supervisor_node_get_from_proto_def to retrieve the internal node * fix def name * fix proto names in test * initialize struct members to NULL * initialize node proto_info to NULL * update expected proto hierarchy * increment loop counter * alternate method of determining the actual parameter of a field * initialize field to NULL * fix SolidProtoHierarchyBase * use correct parameter retrieval method * use internal=true for proto parameter lookups * fix bug when retrieving the first field of a proto * fix base node field query * refresh internal node reference after regeneration * fix internal_proto nullity check * fix base node field lookup error messages * fix retrieval of internal node fields * don't expect changes in read-only fields * use correct check value in read-only assertion * don't check non-existent parameters * fix array in if condition * add wb_supervisor_proto_is_derived * fix wb_proto_is_derived * update c++ api * fix variable name conflict * add Node::getProto and update style * update java api * add todo * update python api * update the matlab api * use camelCase in python api * add wb_supervisor_proto_is_derived to python and matlab apis * update WbLanguage.cpp * rename methods for clarity * fix circular dependency * fix other circular dependency * unset allow_search_in_proto * swap field read only logic * update proto_get_parent method name * update references to renamed supervisor methods * update ros api * run clang-format * fix cppcheck warnings * fix Controller.def method names * fix function name * python formatting * ros fixes * run clang-format * more ros fixes * pin empy version * bump webots_ros * bump webots_ros * bump webots_ros * bump webots_ros * fix lua syntax * account for proto regeneration in test suite * verify field integrity before checking lookup_field * run clang-format * don't return invalid proto references * fix FieldImportBase proto definition * fix race condition (see long commit message) Before this commit, the code would update one of the fields, wait one timestep, and verify the change while updating the next field (two separate controllers). However, because the field updates regenerated the node, this led some references used in the check to be invalidated during the check. This was not an issue before, because no internal fields were used in the previous version of the test (so nothing was invalidated). This commit adds an additional step to each field update so that the check gets its own timestep. This means that the node is no longer regenerated while its being validated. * use snake_case in supervisor_notify_import_remove_mf * get rid of FieldImportBase * use spaces instead of tabs * cleanup proto map * run clang-format * remove const for consistency * make wb_supervisor_proto_get_type_name(NULL) return "" for consistency * update documentation * fix references to old proto field access methods * run clang-format * fix proto regeneration check * update the changelog * bug fixes * always get fields by name * run clang-format * remove explicit field name array size * Bump actions/download-artifact from 3 to 4.1.7 in /.github/workflows (#6652) Bumps [actions/download-artifact](https://github.com/actions/download-artifact) from 3 to 4.1.7. - [Release notes](https://github.com/actions/download-artifact/releases) - [Commits](https://github.com/actions/download-artifact/compare/v3...v4.1.7) --- updated-dependencies: - dependency-name: actions/download-artifact dependency-type: direct:production ... Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> * add comment * Bump actions/upload-artifact to v4 (#6654) * include proto id in field query when processing value get requests * check for proto parent validity instead of just NULL (this shouldn't be necessary because if the parent ref is invalidated, the child should've been as well, but it's good practice) * fix variable name * add changelog entry for matlab method signature change * fix test assert message * formatting * fix types in supervisor documentation Co-authored-by: Olivier Michel * add missing references to wb_supervisor_node_get_proto * fix ros service ids in documentation * add wb_supervisor_node_get_proto to Controller.def --------- Signed-off-by: dependabot[bot] Co-authored-by: dependabot[bot] <49699333+dependabot[bot]@users.noreply.github.com> Co-authored-by: Dean Brettle Co-authored-by: Olivier Michel --- .github/workflows/test_suite_linux.yml | 2 +- .../workflows/test_suite_linux_develop.yml | 2 +- docs/reference/changelog-r2020.md | 10 +- docs/reference/changelog-r2024.md | 7 + docs/reference/supervisor.md | 278 +++++++++-- include/controller/c/webots/supervisor.h | 15 +- include/controller/c/webots/types.h | 1 + include/controller/cpp/webots/Field.hpp | 1 + include/controller/cpp/webots/Node.hpp | 9 +- include/controller/cpp/webots/Proto.hpp | 46 ++ lib/controller/matlab/.gitignore | 14 +- lib/controller/python/controller/__init__.py | 3 +- lib/controller/python/controller/field.py | 22 +- lib/controller/python/controller/node.py | 39 +- lib/controller/python/controller/proto.py | 60 +++ .../default/controllers/ros/RosSupervisor.cpp | 106 +++- .../default/controllers/ros/RosSupervisor.hpp | 30 ++ resources/webots_ros | 2 +- src/controller/c/Controller.def | 16 +- src/controller/c/messages.h | 1 + src/controller/c/supervisor.c | 453 ++++++++++++++++-- src/controller/cpp/Field.cpp | 5 + src/controller/cpp/Makefile | 1 + src/controller/cpp/Node.cpp | 17 +- src/controller/cpp/Proto.cpp | 73 +++ src/controller/cpp/Supervisor.cpp | 1 + src/controller/java/Makefile | 1 + src/controller/java/controller.i | 89 ++++ src/controller/matlab/mgenerate.py | 18 +- src/webots/Makefile | 1 + src/webots/core/WbLanguage.cpp | 17 +- .../nodes/utils/WbSupervisorUtilities.cpp | 202 ++++++-- .../nodes/utils/WbSupervisorUtilities.hpp | 6 + src/webots/vrml/WbNode.cpp | 42 +- src/webots/vrml/WbNode.hpp | 4 + src/webots/vrml/WbNodeProtoInfo.cpp | 50 ++ src/webots/vrml/WbNodeProtoInfo.hpp | 52 ++ .../supervisor_field/supervisor_field.c | 45 +- .../supervisor_import_remove_mf.c | 16 +- .../supervisor_notify_import_remove_mf.c | 71 ++- .../controllers/supervisor_proto/.gitignore | 1 + .../api/controllers/supervisor_proto/Makefile | 25 + .../supervisor_proto/supervisor_proto.c | 150 ++++++ .../supervisor_proto_fields/.gitignore | 1 + .../supervisor_proto_fields/Makefile | 25 + .../supervisor_proto_fields.c | 287 +++++++++++ tests/api/protos/SolidProtoHierarchy.proto | 26 + .../api/protos/SolidProtoHierarchyBase.proto | 17 + .../protos/SolidProtoHierarchyInternal.proto | 20 + tests/api/worlds/supervisor_proto.wbt | 45 ++ tests/lib/ts_assertion.h | 4 +- .../template_indirect_field_access.c | 2 +- 52 files changed, 2220 insertions(+), 211 deletions(-) create mode 100644 include/controller/cpp/webots/Proto.hpp create mode 100644 lib/controller/python/controller/proto.py create mode 100644 src/controller/cpp/Proto.cpp create mode 100644 src/webots/vrml/WbNodeProtoInfo.cpp create mode 100644 src/webots/vrml/WbNodeProtoInfo.hpp create mode 100644 tests/api/controllers/supervisor_proto/.gitignore create mode 100644 tests/api/controllers/supervisor_proto/Makefile create mode 100644 tests/api/controllers/supervisor_proto/supervisor_proto.c create mode 100644 tests/api/controllers/supervisor_proto_fields/.gitignore create mode 100644 tests/api/controllers/supervisor_proto_fields/Makefile create mode 100644 tests/api/controllers/supervisor_proto_fields/supervisor_proto_fields.c create mode 100644 tests/api/protos/SolidProtoHierarchy.proto create mode 100644 tests/api/protos/SolidProtoHierarchyBase.proto create mode 100644 tests/api/protos/SolidProtoHierarchyInternal.proto create mode 100644 tests/api/worlds/supervisor_proto.wbt diff --git a/.github/workflows/test_suite_linux.yml b/.github/workflows/test_suite_linux.yml index 130d5cb91b4..c264ac64e8b 100644 --- a/.github/workflows/test_suite_linux.yml +++ b/.github/workflows/test_suite_linux.yml @@ -160,7 +160,7 @@ jobs: run: | export WEBOTS_HOME=$PWD/artifact/webots export ROS_DISTRO=${{ matrix.ROS_DISTRO }} - python -m pip install rospkg catkin_pkg empy defusedxml netifaces + python -m pip install rospkg catkin_pkg empy==3.3.4 defusedxml netifaces Xvfb :99 & export DISPLAY=:99 ./tests/ros.sh diff --git a/.github/workflows/test_suite_linux_develop.yml b/.github/workflows/test_suite_linux_develop.yml index d365ab95b66..ebb174fd025 100644 --- a/.github/workflows/test_suite_linux_develop.yml +++ b/.github/workflows/test_suite_linux_develop.yml @@ -153,7 +153,7 @@ jobs: run: | export WEBOTS_HOME=$PWD/artifact/webots export ROS_DISTRO=${{ matrix.ROS_DISTRO }} - python -m pip install rospkg catkin_pkg empy defusedxml netifaces + python -m pip install rospkg catkin_pkg empy==3.3.4 defusedxml netifaces Xvfb :99 & export DISPLAY=:99 ./tests/ros.sh diff --git a/docs/reference/changelog-r2020.md b/docs/reference/changelog-r2020.md index 0f68b0a77b3..b1db20ba47b 100644 --- a/docs/reference/changelog-r2020.md +++ b/docs/reference/changelog-r2020.md @@ -22,15 +22,15 @@ Released on September 1st, 2020. - Remove scaling factor in matrix returned by [`wb_supervisor_node_get_orientation`](supervisor.md#wb_supervisor_node_get_orientation) ([#2112](https://github.com/cyberbotics/webots/pull/2112)). - Fixed conversion of identity matrix to quaternion in ROS API ([#2112](https://github.com/cyberbotics/webots/pull/2112)). - Fixed header stamps of the topics published by the ROS controller ([#2127](https://github.com/cyberbotics/webots/pull/2127)). - - Fixed the [`camera/recognition_objects`](camera.md#wb_camera_recognition_get_objects) ROS topic not published when the [`camera/image`](camera.md#wb_camera_recognition_get_objects) topic has no subscriber ([#2115](https://github.com/cyberbotics/webots/pull/2115)). - - **macOS: Removed the `ros` controller**, the [custom Python ROS controller](https://www.cyberbotics.com/doc/guide/using-ros#custom-ros-controller) should be used instead ([#2053](https://github.com/cyberbotics/webots/pull/2053)). + - Fixed the [`camera/recognition_objects`](camera.md#wb_camera_recognition_get_objects) ROS topic not published when the [`camera/image`](camera.md#wb_camera_recognition_get_objects) topic has no subscriber ([#2115](https://github.com/cyberbotics/webots/pull/2115)). + - **macOS: Removed the `ros` controller**, the [custom Python ROS controller](https://www.cyberbotics.com/doc/guide/using-ros#custom-ros-controller) should be used instead ([#2053](https://github.com/cyberbotics/webots/pull/2053)). - Fixed DEF node not found if defined in PROTO default parameter value ([#2107](https://github.com/cyberbotics/webots/pull/2107)). - Fixed crash occurring with some PROTO nodes when modifying fields from the scene tree that trigger the PROTO model regeneration ([#2100](https://github.com/cyberbotics/webots/pull/2100)). - Fixed field changes not applied in case of nested [PROTO](proto.md) nodes ([#2063](https://github.com/cyberbotics/webots/pull/2063)). - Windows: Fixed generation of procedural PROTO nodes using the `gd` module ([#2070](https://github.com/cyberbotics/webots/pull/2070)). - Fixed the [vision](../guide/samples-howto.md#vision-wbt) sample simulation ([#2143](https://github.com/cyberbotics/webots/pull/2143)). - Fixed bugs in streaming server protocol and added support for X3D/MJPEG mode selection in simulation server ([#2077](https://github.com/cyberbotics/webots/pull/2077)). - - Linux: Fixed the execution of robot controllers with firejail ([#2071](https://github.com/cyberbotics/webots/pull/2071)). + - Linux: Fixed the execution of robot controllers with firejail ([#2071](https://github.com/cyberbotics/webots/pull/2071)). - Fixed the `roadBorderWidth` field of the `HelicoidalRoadSegment` PROTO node ([#2099](https://github.com/cyberbotics/webots/pull/2099)). - Fixed the `near` field of the `Robotino3Webcam` [Camera](camera.md) ([#2051](https://github.com/cyberbotics/webots/pull/2051)). - Fixed orientation of the [Lights](light.md) in the `robotino3` world ([#2051](https://github.com/cyberbotics/webots/pull/2051)). @@ -51,7 +51,7 @@ Released on July 29th, 2020. - New Features - Added a [Mesh](mesh.md) node allowing to use external 3D file in Webots ([#1419](https://github.com/cyberbotics/webots/pull/1419)). - Added the possibility to import [3D Studio mesh](https://wiki.fileformat.com/3d/3ds), [Blender](https://www.blender.org/), [Biovision Hierarchy](https://en.wikipedia.org/wiki/Biovision_Hierarchy), [Collada](https://en.wikipedia.org/wiki/COLLADA), [Filmbox](https://wiki.fileformat.com/3d/fbx/), [STL](https://en.wikipedia.org/wiki/STL_(file_format)), [Wavefront](https://wiki.fileformat.com/3d/obj), [X3D](https://www.web3d.org/getting-started-x3d) files in Webots ([#1463](https://github.com/cyberbotics/webots/pull/1463)). - - Added two new functions to get internal PROTO node fields: [`wb_supervisor_node_get_from_proto_def`](supervisor.md#wb_supervisor_node_get_from_proto_def) and [`wb_supervisor_node_get_proto_field`](supervisor.md#wb_supervisor_node_get_proto_field) ([#1331](https://github.com/cyberbotics/webots/pull/1331)). + - Added two new functions to get internal PROTO node fields: [`wb_supervisor_node_get_from_proto_def`](supervisor.md#wb_supervisor_node_get_from_proto_def) and [`wb_supervisor_node_get_proto_field`](supervisor.md#wb_supervisor_node_get_base_node_field) ([#1331](https://github.com/cyberbotics/webots/pull/1331)). - Added the `mjpeg` web streaming mode ([#1352](https://github.com/cyberbotics/webots/pull/1352)). - Exposed global texture maximum filtering as a parameter in the Webots preferences ([#1851](https://github.com/cyberbotics/webots/pull/1851)). - Added a [`wb_robot_get_urdf`](robot.md#wb_robot_get_urdf) function to the [Robot](robot.md) node which allows URDF export ([#1706](https://github.com/cyberbotics/webots/pull/1706)). @@ -105,7 +105,7 @@ Released on July 29th, 2020. - Fixed crash when setting an invalid value to a field that triggers the parent PROTO regeneration ([#1868](https://github.com/cyberbotics/webots/pull/1868)). - Fixed crash when converting a PROTO node to Base node(s) if contained in a field that triggers the parent PROTO regeneration ([#1868](https://github.com/cyberbotics/webots/pull/1868)). - Fixed crash when deleting a node contained in a field that triggers the parent PROTO regeneration ([#1868](https://github.com/cyberbotics/webots/pull/1868)). - - Fixed crash occurring when reloading or resetting a simulation containing a [Display](display.md) device ([#1865](https://github.com/cyberbotics/webots/pull/1865)). + - Fixed crash occurring when reloading or resetting a simulation containing a [Display](display.md) device ([#1865](https://github.com/cyberbotics/webots/pull/1865)). - Fixed crash with Python [`RangeFinder.rangeImageGetDepth`](rangefinder.md#wb_range_finder_image_get_depth) function ([#1858](https://github.com/cyberbotics/webots/pull/1858)). - Fixed mismatch between the bounding object and visual shape of the [UnevenTerrain](https://www.cyberbotics.com/doc/guide/object-floors#uneventerrain), **and removed the `textureScale` field** ([#1792](https://github.com/cyberbotics/webots/pull/1792)). - Fixed crash when using a [Normal](normal.md) node in a PROTO node ([#1813](https://github.com/cyberbotics/webots/pull/1813)). diff --git a/docs/reference/changelog-r2024.md b/docs/reference/changelog-r2024.md index ca4b1c4f26f..96f63127e2f 100644 --- a/docs/reference/changelog-r2024.md +++ b/docs/reference/changelog-r2024.md @@ -7,6 +7,13 @@ Released on December **th, 2023. - Removed support for macOS 11 "Big Sur" and added support for macOS 14 "Sonoma" ([#6580](https://github.com/cyberbotics/webots/pull/6580)). - Added the `indirectFieldAccess` tag to allow the `fields` variable to be used in proto templates without referencing a specific field ([#6614](https://github.com/cyberbotics/webots/pull/6614)). - Added a method to include all subtypes of a node type in a PROTO field restriction ([#6574](https://github.com/cyberbotics/webots/pull/6574)). + - Improved the node field query api ([#6613](https://github.com/cyberbotics/webots/issues/6613)). + - **Renamed the `wb_supervisor_node_get_proto_*` methods to `wb_supervisor_node_get_base_node_*`** + - **Renamed the `proto` field in the `supervisor_node_get_field_*` ROS services to `queryBaseNode`** + - Added the `WbProtoRef` type to the supervisor API. + - Added the ability to query the internal structure and fields of a proto node. + - Added the ability to query the field in the scene tree that corresponds to a proto internal field. + - Fixed the method signature of `wb_supervisor_node_get_number_of_fields` in MATLAB. - Removed support for Lua as a PROTO scripting language ([#6642](https://github.com/cyberbotics/webots/pull/6642)). - Enhancements - Improved the image range of the rotating [Lidar](lidar.md) ([#6324](https://github.com/cyberbotics/webots/pull/6324)). diff --git a/docs/reference/supervisor.md b/docs/reference/supervisor.md index 886bdfa9d26..863ec8086b1 100644 --- a/docs/reference/supervisor.md +++ b/docs/reference/supervisor.md @@ -167,6 +167,7 @@ If no node is currently selected, the function returns NULL. #### `wb_supervisor_node_get_id` #### `wb_supervisor_node_get_parent_node` #### `wb_supervisor_node_is_proto` +#### `wb_supervisor_node_get_proto` #### `wb_supervisor_node_get_from_proto_def` %tab-component "language" @@ -180,6 +181,7 @@ const char *wb_supervisor_node_get_def(WbNodeRef node); int wb_supervisor_node_get_id(WbNodeRef node); WbNodeRef wb_supervisor_node_get_parent_node(WbNodeRef node); bool wb_supervisor_node_is_proto(WbNodeRef node); +WbProtoRef wb_supervisor_node_get_proto(WbNodeRef node); WbNodeRef wb_supervisor_node_get_from_proto_def(WbNodeRef node, const char *def); ``` @@ -196,6 +198,7 @@ namespace webots { std::string getDef() const; Node *getParentNode() const; bool isProto() const; + Proto *getProto() const; Node *getFromProtoDef(const std::string &name); // ... } @@ -214,6 +217,7 @@ class Node: def getDef(self): def getParentNode(self): def isProto(self): + def getProto(self): def getFromProtoDef(self, name): # ... ``` @@ -230,6 +234,7 @@ public class Node { public String getDef(); public Node getParentNode(); public boolean isProto(); + public Proto getProto(); public Node getFromProtoDef(String name); // ... } @@ -244,6 +249,7 @@ id = wb_supervisor_node_get_id(node) s = wb_supervisor_node_get_def(node) node = wb_supervisor_node_get_parent_node(node) b = wb_supervisor_node_is_proto(node) +proto = wb_supervisor_node_get_proto(node) node = wb_supervisor_node_get_from_proto_def(node, 'def') ``` @@ -257,6 +263,7 @@ node = wb_supervisor_node_get_from_proto_def(node, 'def') | `/supervisor/node/get_def` | `service` | `webots_ros::node_get_name` | `uint64 node`
`---`
`string name` | | `/supervisor/node/get_parent_node` | `service` | `webots_ros::node_get_parent_node` | `uint64 node`
`---`
`uint64 node` | | `/supervisor/node/is_proto` | `service` | `webots_ros::node_is_proto` | `uint64 node`
`---`
`bool value` | +| `/supervisor/node/get_proto` | `service` | `webots_ros::node_get_proto` | `uint64 node`
`---`
`uint64 proto` | | `/supervisor/get_from_def` | `service` | `webots_ros::supervisor_get_from_def` | `string name`
`uint64 proto`
`---`
`uint64 node` | %tab-end @@ -274,6 +281,9 @@ The `wb_supervisor_node_get_id` function retrieves the unique identifier of the The `wb_supervisor_node_get_parent_node` function retrieves the reference to the direct parent node of the node given in parameter. +The `wb_supervisor_node_get_proto` function the instance of the proto type corresponding to the given node. +If the node is not a PROTO node, the function returns NULL. + The `wb_supervisor_node_is_proto` function returns `true` if the node given in the argument is a [PROTO node](proto.md). The `wb_supervisor_node_get_from_proto_def` function returns a handle to a node defined in the [PROTO body](proto-definition.md) of the specified node from its DEF name. @@ -744,9 +754,9 @@ A file with the equivalent content can be produced in the Webots user interface #### `wb_supervisor_node_get_field` #### `wb_supervisor_node_get_field_by_index` #### `wb_supervisor_node_get_number_of_fields` -#### `wb_supervisor_node_get_proto_field` -#### `wb_supervisor_node_get_proto_field_by_index` -#### `wb_supervisor_node_get_proto_number_of_fields` +#### `wb_supervisor_node_get_base_node_field` +#### `wb_supervisor_node_get_base_node_field_by_index` +#### `wb_supervisor_node_get_number_of_base_node_fields` %tab-component "language" @@ -758,9 +768,9 @@ A file with the equivalent content can be produced in the Webots user interface WbFieldRef wb_supervisor_node_get_field(WbNodeRef node, const char *field_name); WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, int index); int wb_supervisor_node_get_number_of_fields(WbNodeRef node); -WbFieldRef wb_supervisor_node_get_proto_field(WbNodeRef node, const char *field_name); -WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, const char *field_name); -int wb_supervisor_node_get_proto_number_of_fields(WbNodeRef node, int index); +WbFieldRef wb_supervisor_node_get_base_node_field(WbNodeRef node, const char *field_name); +WbFieldRef wb_supervisor_node_get_base_node_field_by_index(WbNodeRef node, int index); +int wb_supervisor_node_get_number_of_base_node_fields(WbNodeRef node); ``` %tab-end @@ -775,9 +785,9 @@ namespace webots { Field *getField(const std::string &fieldName) const; Field *getFieldByIndex(int index) const; int getNumberOfFields() const; - Field *getProtoField(const std::string &fieldName) const; - Field *getProtoFieldByIndex(int index) const; - int getProtoNumberOfFields() const; + Field *getBaseNodeField(const std::string &fieldName) const; + Field *getBaseNodeFieldByIndex(int index) const; + int getNumberOfBaseNodeFields() const; // ... } } @@ -793,10 +803,10 @@ from controller import Node class Node: def getField(self, fieldName): def getFieldByIndex(self, index): - def getNumberOfFields(): - def getProtoField(self, fieldName): - def getProtoFieldByIndex(self, index): - def getProtoNumberOfFields(): + def getNumberOfFields(self): + def getBaseNodeField(self, fieldName): + def getBaseNodeFieldByIndex(self, index): + def getNumberOfBaseNodeFields(self): # ... ``` @@ -811,9 +821,9 @@ public class Node { public Field getField(String fieldName); public Field getFieldByIndex(int index); public int getNumberOfFields(); - public Field getProtoField(String fieldName); - public Field getProtoFieldByIndex(int index); - public int getProtoNumberOfFields(); + public Field getBaseNodeField(String fieldName); + public Field getBaseNodeFieldByIndex(int index); + public int getNumberOfBaseNodeFields(); // ... } ``` @@ -826,9 +836,9 @@ public class Node { field = wb_supervisor_node_get_field(node, 'field_name') field = wb_supervisor_node_get_field_by_index(node, index) size = wb_supervisor_node_get_number_of_fields(node) -field = wb_supervisor_node_get_proto_field(node, 'field_name') -field = wb_supervisor_node_get_proto_field_by_index(node, index) -size = wb_supervisor_node_get_proto_number_of_fields(node) +field = wb_supervisor_node_get_base_node_field(node, 'field_name') +field = wb_supervisor_node_get_base_node_field_by_index(node, index) +size = wb_supervisor_node_get_number_of_base_node_fields(node) ``` %tab-end @@ -837,9 +847,9 @@ size = wb_supervisor_node_get_proto_number_of_fields(node) | name | service/topic | data type | data type definition | | --- | --- | --- | --- | -| `/supervisor/node/get_field` | `service` | `webots_ros::node_get_field` | `uint64 node`
`string fieldName`
`bool proto`
`---`
`uint64 field` | -| `/supervisor/node/get_field_by_index` | `service` | `webots_ros::node_get_field_by_index` | `uint64 node`
`int index`
`bool proto`
`---`
`uint64 field` | -| `/supervisor/node/get_number_of_fields` | `service` | `webots_ros::node_get_number_of_fields` | `uint64 node`
`bool proto`
`---`
`uint32 value` | +| `/supervisor/node/get_field` | `service` | `webots_ros::node_get_field` | `uint64 node`
`string fieldName`
`bool queryBaseNode`
`---`
`uint64 field` | +| `/supervisor/node/get_field_by_index` | `service` | `webots_ros::node_get_field_by_index` | `uint64 node`
`int index`
`bool queryBaseNode`
`---`
`uint64 field` | +| `/supervisor/node/get_number_of_fields` | `service` | `webots_ros::node_get_number_of_fields` | `uint64 node`
`bool queryBaseNode`
`---`
`uint32 value` | %tab-end @@ -849,22 +859,22 @@ size = wb_supervisor_node_get_proto_number_of_fields(node) *get a field reference from a node* -The `wb_supervisor_node_get_field` function retrieves a handler to a node field. +The `wb_supervisor_node_get_field` function retrieves a handler to a node field in the scene tree. The field is specified by its name in `field_name` and the `node` it belongs to. It can be a single field (SF) or a multiple field (MF). -If no such field name exists for the specified node or the field is an internal field of a PROTO, the return value is NULL. +If no such field name exists for the specified node in the scene tree, the return value is NULL. Otherwise, it returns a handler to a field. > **Note**: The `wb_supervisor_node_get_field` function will return a valid field handler if the field corresponding to the field name is an hidden field. -If the field is an internal field of a PROTO, the `wb_supervisor_node_get_proto_field` function should be used instead. +If a field is defined for the base (Webots) type of a node but does not appear in the scene tree due to being an internal field of a PROTO, the `wb_supervisor_node_get_base_node_field` function can be used instead. Field handlers can also be retrieved by index using the `wb_supervisor_node_get_field_by_index` function where the field is specified by its `index` and the the `node` it belongs to. Valid `index` values should be positive and lower than the number of fields returned by `wb_supervisor_node_get_number_of_fields`. -If the arguments are not valid, `wb_supervisor_node_get_field_by_index` returns NULL and `wb_supervisor_node_get_number_of_fields` return -1. -To retrieved an internal field of a PROTO, the `wb_supervisor_node_get_proto_field_by_index` and `wb_supervisor_node_get_proto_number_of_fields` should be used instead. +If the arguments are not valid, `wb_supervisor_node_get_field_by_index` returns NULL and `wb_supervisor_node_get_number_of_fields` returns -1. +To retrieve an internal field of a PROTO, the `wb_supervisor_node_get_base_node_field_by_index` and `wb_supervisor_node_get_number_of_base_node_fields` should be used instead. -> **Note**: fields retrieved with the `wb_supervisor_node_get_proto_field` and `wb_supervisor_node_get_proto_field_by_index` functions are read-only. Which means that it is not possible to change them using any of the [`wb_supervisor_field_set_*`](#wb_supervisor_field_set_sf_bool) functions. +> **Note**: fields retrieved with the `wb_supervisor_node_get_base_node_field` and `wb_supervisor_node_get_base_node_by_index` functions are read-only. Which means that it is not possible to change them using any of the [`wb_supervisor_field_set_*`](#wb_supervisor_field_set_sf_bool) functions. @@ -3123,6 +3133,7 @@ Both `wb_supervisor_animation_start_recording` and `wb_supervisor_animation_stop #### `wb_supervisor_field_get_type` #### `wb_supervisor_field_get_type_name` #### `wb_supervisor_field_get_count` +#### `wb_supervisor_field_get_actual_field` #### `wb_supervisor_field_enable_sf_tracking` #### `wb_supervisor_field_disable_sf_tracking` @@ -3143,6 +3154,7 @@ const char *wb_supervisor_field_get_name(WbFieldRef field); WbFieldType wb_supervisor_field_get_type(WbFieldRef field); const char *wb_supervisor_field_get_type_name(WbFieldRef field); int wb_supervisor_field_get_count(WbFieldRef field); +WbFieldRef wb_supervisor_field_get_actual_field(WbFieldRef field); void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_period); void wb_supervisor_field_disable_sf_tracking(WbFieldRef field); ``` @@ -3166,6 +3178,7 @@ namespace webots { Type getType() const; std::string getTypeName() const; int getCount() const; + WbFieldRef getActualField() const; void enableSFTracking(int samplingPeriod); void disableSFTracking(); // ... @@ -3189,6 +3202,7 @@ class Field: def getType(self): def getTypeName(self): def getCount(self): + def getActualField(self): def enableSFTracking(self, samplingPeriod): def disableSFTracking(self): # ... @@ -3210,6 +3224,7 @@ public class Field { public int getType(); public String getTypeName(); public int getCount(); + public Field getActualField(); public void enableSFTracking(int samplingPeriod); public void disableSFTracking(); // ... @@ -3229,6 +3244,7 @@ name = wb_supervisor_field_get_name(field) type = wb_supervisor_field_get_type(field) name = wb_supervisor_field_get_type_name(field) count = wb_supervisor_field_get_count(field) +actual_field = wb_supervisor_field_get_actual_field(field) wb_supervisor_field_enable_sf_tracking(field, sampling_period) wb_supervisor_field_disable_sf_tracking(field) ``` @@ -3243,6 +3259,7 @@ wb_supervisor_field_disable_sf_tracking(field) | `/supervisor/field/get_type` | `service` | `webots_ros::field_get_type` | `uint64 node`
`---`
`int8 success` | | `/supervisor/field/get_type_name` | `service` | `webots_ros::field_get_name` | `uint64 field`
`---`
`string name` | | `/supervisor/field/get_count` | `service` | `webots_ros::field_get_count` | `uint64 field`
`---`
`int32 count` | +| `/supervisor/field/get_actual_field` | `service` | `webots_ros::field_get_actual_field` | `uint64 field`
`---`
`uint64 field` | | `/supervisor/field/enable_sf_tracking` | `service` | `webots_ros::field_enable_sf_tracking` | `uint64 field`
`int32 sampling_period`
`---`
`int8 sampling_period` | | `/supervisor/field/disable_sf_tracking` | `service` | `webots_ros::field_disable_sf_tracking` | `uint64 field`
`---`
`int8 success` | @@ -3270,6 +3287,11 @@ The `wb_supervisor_field_get_count` function returns the number of items of a mu If a single field (SF) or NULL is passed as an argument to this function, it returns -1. Hence, this function can also be used to test if a field is MF (like `WB_MF_INT32`) or SF (like `WB_SF_BOOL`). +The `wb_supervisor_field_get_actual_field` function returns the field in the scene tree that is associated with the given field. +This allows you to retrieve a modifiable field reference from an internal field reference. (e.g., a field reference returned by [`wb_supervisor_node_get_base_node_field`](#wb_supervisor_node_get_base_node_field) or [`wb_supervisor_proto_get_field`](#wb_supervisor_proto_get_field)). +If the argument is invalid or doesn't have an associated field in the tree, the function returns NULL. +If the function does not return NULL, the returned field is guaranteed to be modifiable. + The `wb_supervisor_field_enable_sf_tracking` function forces Webots to stream field data to the controller. It improves the performance as the controller by default uses a request-response pattern to get data from the field. The `sampling_period` argument determines how often the field data should be sent to the controller. @@ -4012,6 +4034,206 @@ rootChildrenField.importMFNodeFromString(4, 'DEF MY_ROBOT Robot { controller "my --- +#### `wb_supervisor_proto_get_type_name` +#### `wb_supervisor_proto_is_derived` +#### `wb_supervisor_proto_get_parent` + +%tab-component "language" + +%tab "C" + +```c +#include + +const char *wb_supervisor_proto_get_type_name(WbProtoRef proto); +bool wb_supervisor_proto_is_derived(WbProtoRef proto, const char *type_name); +WbProtoRef wb_supervisor_proto_get_parent(WbProtoRef proto); +``` + +%tab-end + +%tab "C++" + +```cpp +#include + +namespace webots { + class Proto { + std::string getTypeName(); + bool isDerived(); + Node getParent(); + // ... + } +} +``` + +%tab-end + +%tab "Python" + +```python +from controller import Proto + +class Proto: + def getTypeName(self): + def isDerived(self): + def getParent(self): + # ... +``` + +%tab-end + +%tab "Java" + +```java +import com.cyberbotics.webots.controller.Proto; + +public class Proto { + public String getTypeName(); + public boolean isDerived(); + public Node getParent(); + // ... +} +``` + +%tab-end + +%tab "MATLAB" + +```MATLAB +type_name = wb_supervisor_proto_get_type_name(proto) +is_derived = wb_supervisor_proto_is_derived(proto) +parent = wb_supervisor_proto_get_parent(proto) +``` + +%tab-end + +%tab "ROS" + +| name | service/topic | data type | data type definition | +| --- | --- | --- | --- | +| `/supervisor/proto/get_type_name` | `service` | `webots_ros::proto_get_type_name` | `uint64 proto`
`---`
`string value` | +| `/supervisor/proto/is_derived` | `service` | `webots_ros::proto_is_derived` | `uint64 proto`
`---`
`bool value` | +| `/supervisor/proto/get_parent` | `service` | `webots_ros::proto_get_parent` | `uint64 proto`
`---`
`uint64 proto` | + +%tab-end + +%end + +##### Description + +*get information about a proto instance* + +The `wb_supervisor_proto_get_type` function returns the type name of a proto instance as defined in the PROTO file. +If the argument is NULL, the function returns the empty string. + +The `wb_supervisor_proto_is_derived` function returns true if the proto instance is derived from another proto type. + +The `wb_supervisor_proto_get_parent` function returns the instance of the parent proto type corresponding to the given proto instance. + +--- + +#### `wb_supervisor_proto_get_field` +#### `wb_supervisor_proto_get_field_by_index` +#### `wb_supervisor_proto_get_number_of_fields` + +%tab-component "language" + +%tab "C" + +```c +#include + +WbFieldRef wb_supervisor_proto_get_field(WbProtoRef proto, const char *field_name); +WbFieldRef wb_supervisor_proto_get_field_by_index(WbProtoRef proto, int index); +int wb_supervisor_proto_get_number_of_fields(WbProtoRef proto); +``` + +%tab-end + +%tab "C++" + +```cpp +#include + +namespace webots { + class Proto { + Field getField(const std::string &fieldName); + Field getFieldByIndex(int index); + int getNumberOfFields(); + // ... + } +} +``` + +%tab-end + +%tab "Python" + +```python +from controller import Proto + +class Proto: + def getField(self, fieldName): + def getFieldByIndex(self, index): + def getNumberOfFields(self): + # ... +``` + +%tab-end + +%tab "Java" + +```java +import com.cyberbotics.webots.controller.Proto; + +public class Proto { + public Field getField(String fieldName); + public Field getFieldByIndex(int index); + public int getNumberOfFields(); + // ... +} +``` + +%tab-end + +%tab "MATLAB" + +```MATLAB +field = wb_supervisor_proto_get_field(proto, 'field_name') +field = wb_supervisor_proto_get_field_by_index(proto, index) +number_of_fields = wb_supervisor_proto_get_number_of_fields(proto) +``` + +%tab-end + +%tab "ROS" + +| name | service/topic | data type | data type definition | +| --- | --- | --- | --- | +| `/supervisor/proto/get_field` | `service` | `webots_ros::proto_get_field` | `uint64 proto`
`string fieldName`
`---`
`uint64 field` | +| `/supervisor/proto/get_field_by_index` | `service` | `webots_ros::proto_get_field_by_index` | `uint64 proto`
`int32 index`
`---`
`uint64 field` | +| `/supervisor/proto/get_number_of_fields` | `service` | `webots_ros::proto_get_number_of_fields` | `uint64 proto`
`---`
`int32 value` | + +%tab-end + +%end + +##### Description + +*get information about a proto instance's fields* + +The `wb_supervisor_proto_get_field` function returns the field of a proto instance corresponding to the given field name. +If the field does not exist or the provided proto is invalid, the function returns NULL. + +The `wb_supervisor_proto_get_field_by_index` function returns the field of a proto instance corresponding to the given index. +If the field does not exist or the provided proto is invalid, the function returns NULL. + +The `wb_supervisor_proto_get_number_of_fields` function returns the number of fields of a proto instance. +If the provided proto is invalid, the function returns -1. + +--- + #### `wb_supervisor_virtual_reality_headset_is_used` #### `wb_supervisor_virtual_reality_headset_get_position` #### `wb_supervisor_virtual_reality_headset_get_orientation` diff --git a/include/controller/c/webots/supervisor.h b/include/controller/c/webots/supervisor.h index c2b71a6e1a6..ccb01222b3b 100644 --- a/include/controller/c/webots/supervisor.h +++ b/include/controller/c/webots/supervisor.h @@ -97,13 +97,14 @@ WbNodeType wb_supervisor_node_get_type(WbNodeRef node); WbFieldRef wb_supervisor_node_get_field(WbNodeRef node, const char *field_name); WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, const int index); int wb_supervisor_node_get_number_of_fields(WbNodeRef node); -WbFieldRef wb_supervisor_node_get_proto_field(WbNodeRef node, const char *field_name); -WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, int index); -int wb_supervisor_node_get_proto_number_of_fields(WbNodeRef node); +WbFieldRef wb_supervisor_node_get_base_node_field(WbNodeRef node, const char *field_name); +WbFieldRef wb_supervisor_node_get_base_node_field_by_index(WbNodeRef node, int index); +int wb_supervisor_node_get_number_of_base_node_fields(WbNodeRef node); void wb_supervisor_node_remove(WbNodeRef node); void wb_supervisor_node_save_state(WbNodeRef node, const char *state_name); void wb_supervisor_node_load_state(WbNodeRef node, const char *state_name); void wb_supervisor_node_set_joint_position(WbNodeRef node, double position, int index); +WbProtoRef wb_supervisor_node_get_proto(WbNodeRef node); const char *wb_supervisor_node_get_def(WbNodeRef node); const char *wb_supervisor_node_get_type_name(WbNodeRef node); @@ -139,6 +140,7 @@ const char *wb_supervisor_field_get_name(WbFieldRef field); WbFieldType wb_supervisor_field_get_type(WbFieldRef field); const char *wb_supervisor_field_get_type_name(WbFieldRef field); int wb_supervisor_field_get_count(WbFieldRef field); +WbFieldRef wb_supervisor_field_get_actual_field(WbFieldRef field); void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_period); void wb_supervisor_field_disable_sf_tracking(WbFieldRef field); @@ -199,6 +201,13 @@ void wb_supervisor_field_import_mf_node_from_string(WbFieldRef field, int positi void wb_supervisor_field_remove_sf(WbFieldRef field); void wb_supervisor_field_import_sf_node_from_string(WbFieldRef field, const char *node_string); +const char *wb_supervisor_proto_get_type_name(WbProtoRef proto); +bool wb_supervisor_proto_is_derived(WbProtoRef proto); +WbProtoRef wb_supervisor_proto_get_parent(WbProtoRef proto); +WbFieldRef wb_supervisor_proto_get_field(WbProtoRef proto, const char *field_name); +WbFieldRef wb_supervisor_proto_get_field_by_index(WbProtoRef proto, int index); +int wb_supervisor_proto_get_number_of_fields(WbProtoRef proto); + bool wb_supervisor_virtual_reality_headset_is_used(); const double *wb_supervisor_virtual_reality_headset_get_position(); const double *wb_supervisor_virtual_reality_headset_get_orientation(); diff --git a/include/controller/c/webots/types.h b/include/controller/c/webots/types.h index 49a5fe7a7e4..08138b18e1e 100644 --- a/include/controller/c/webots/types.h +++ b/include/controller/c/webots/types.h @@ -36,6 +36,7 @@ typedef unsigned short WbDeviceTag; // identifier of a device typedef struct WbImageStructPrivate *WbImageRef; typedef struct WbMotionStructPrivate *WbMotionRef; typedef struct WbNodeStructPrivate *WbNodeRef; +typedef struct WbProtoInfoStructPrivate *WbProtoRef; typedef struct WbFieldStructPrivate *WbFieldRef; // define "bool" type for C controllers diff --git a/include/controller/cpp/webots/Field.hpp b/include/controller/cpp/webots/Field.hpp index 1e00ebc58cc..3d97e2c43a4 100644 --- a/include/controller/cpp/webots/Field.hpp +++ b/include/controller/cpp/webots/Field.hpp @@ -55,6 +55,7 @@ namespace webots { Type getType() const; std::string getTypeName() const; int getCount() const; + Field *getActualField() const; void enableSFTracking(int samplingPeriod); void disableSFTracking(); diff --git a/include/controller/cpp/webots/Node.hpp b/include/controller/cpp/webots/Node.hpp index a18ba417b89..32c91f471dc 100644 --- a/include/controller/cpp/webots/Node.hpp +++ b/include/controller/cpp/webots/Node.hpp @@ -18,6 +18,7 @@ #define WB_USING_CPP_API #include #include +#include #include "../../c/webots/contact_point.h" #include "../../c/webots/types.h" @@ -27,6 +28,7 @@ namespace webots { typedef WbContactPoint ContactPoint; class Field; + class Proto; class Node { public: typedef enum { @@ -133,13 +135,14 @@ namespace webots { std::string getBaseTypeName() const; Node *getParentNode() const; bool isProto() const; + Proto *getProto() const; Node *getFromProtoDef(const std::string &name) const; int getNumberOfFields() const; - int getProtoNumberOfFields() const; + int getNumberOfBaseNodeFields() const; Field *getField(const std::string &fieldName) const; - Field *getProtoField(const std::string &fieldName) const; + Field *getBaseNodeField(const std::string &fieldName) const; Field *getFieldByIndex(const int index) const; - Field *getProtoFieldByIndex(const int index) const; + Field *getBaseNodeFieldByIndex(const int index) const; const double *getPosition() const; const double *getOrientation() const; const double *getPose() const; diff --git a/include/controller/cpp/webots/Proto.hpp b/include/controller/cpp/webots/Proto.hpp new file mode 100644 index 00000000000..8857afb9cdf --- /dev/null +++ b/include/controller/cpp/webots/Proto.hpp @@ -0,0 +1,46 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// 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 +// +// https://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 PROTO_HPP +#define PROTO_HPP + +#define WB_USING_CPP_API +#include +#include +#include "../../c/webots/types.h" + +namespace webots { + class Field; + class Proto { + public: + std::string getTypeName() const; + bool isDerived() const; + Proto *getParent() const; + Field *getField(const std::string &fieldName) const; + Field *getFieldByIndex(const int index) const; + int getNumberOfFields() const; + + // DO NOT USE THESE FUNCTIONS: THEY ARE RESERVED FOR INTERNAL USE: + static Proto *findProto(WbProtoRef ref); + static void cleanup(); + + private: + Proto(WbProtoRef ref); + ~Proto() {} + + WbProtoRef protoRef; + }; +} // namespace webots + +#endif // PROTO_HPP diff --git a/lib/controller/matlab/.gitignore b/lib/controller/matlab/.gitignore index 00613a243dd..36b5024c07a 100644 --- a/lib/controller/matlab/.gitignore +++ b/lib/controller/matlab/.gitignore @@ -254,6 +254,7 @@ wb_supervisor_animation_stop_recording.m wb_supervisor_export_image.m wb_supervisor_field_disable_sf_tracking.m wb_supervisor_field_enable_sf_tracking.m +wb_supervisor_field_get_actual_field.m wb_supervisor_field_get_count.m wb_supervisor_field_get_mf_bool.m wb_supervisor_field_get_mf_float.m @@ -299,6 +300,8 @@ wb_supervisor_node_enable_contact_points_tracking.m wb_supervisor_node_enable_contact_point_tracking.m wb_supervisor_node_enable_pose_tracking.m wb_supervisor_node_export_string.m +wb_supervisor_node_get_base_node_field.m +wb_supervisor_node_get_base_node_field_by_index.m wb_supervisor_node_get_base_type_name.m wb_supervisor_node_get_contact_point_node.m wb_supervisor_node_get_def.m @@ -309,12 +312,11 @@ wb_supervisor_node_get_from_device.m wb_supervisor_node_get_from_id.m wb_supervisor_node_get_from_proto_def.m wb_supervisor_node_get_id.m +wb_supervisor_node_get_number_of_base_node_fields.m wb_supervisor_node_get_number_of_contact_points.m wb_supervisor_node_get_number_of_fields.m wb_supervisor_node_get_parent_node.m -wb_supervisor_node_get_proto_field.m -wb_supervisor_node_get_proto_field_by_index.m -wb_supervisor_node_get_proto_number_of_fields.m +wb_supervisor_node_get_proto.m wb_supervisor_node_get_root.m wb_supervisor_node_get_selected.m wb_supervisor_node_get_self.m @@ -331,6 +333,12 @@ wb_supervisor_node_save_state.m wb_supervisor_node_set_joint_position.m wb_supervisor_node_set_velocity.m wb_supervisor_node_set_visibility.m +wb_supervisor_proto_get_field.m +wb_supervisor_proto_get_field_by_index.m +wb_supervisor_proto_get_number_of_fields.m +wb_supervisor_proto_get_parent.m +wb_supervisor_proto_get_type_name.m +wb_supervisor_proto_is_derived.m wb_supervisor_simulation_get_mode.m wb_supervisor_simulation_quit.m wb_supervisor_simulation_reset.m diff --git a/lib/controller/python/controller/__init__.py b/lib/controller/python/controller/__init__.py index 61373467d10..20509d5dd02 100644 --- a/lib/controller/python/controller/__init__.py +++ b/lib/controller/python/controller/__init__.py @@ -13,6 +13,7 @@ # limitations under the License. from controller.field import Field # noqa +from controller.proto import Proto # noqa from controller.node import Node, ContactPoint # noqa from controller.ansi_codes import AnsiCodes # noqa from controller.accelerometer import Accelerometer # noqa @@ -52,6 +53,6 @@ __all__ = [ Accelerometer, Altimeter, AnsiCodes, Brake, Camera, CameraRecognitionObject, Compass, Connector, ContactPoint, Display, DistanceSensor, Emitter, Field, GPS, Gyro, InertialUnit, Joystick, Keyboard, LED, Lidar, LidarPoint, LightSensor, Motion, - Motor, Mouse, MouseState, Node, PositionSensor, Radar, RadarTarget, RangeFinder, Receiver, Robot, Skin, Speaker, + Motor, Mouse, MouseState, Node, PositionSensor, Proto, Radar, RadarTarget, RangeFinder, Receiver, Robot, Skin, Speaker, Supervisor, TouchSensor, VacuumGripper ] diff --git a/lib/controller/python/controller/field.py b/lib/controller/python/controller/field.py index 3121cebea46..0ebbfd90fd0 100644 --- a/lib/controller/python/controller/field.py +++ b/lib/controller/python/controller/field.py @@ -41,10 +41,7 @@ class Field: wb.wb_supervisor_field_get_name.restype = ctypes.c_char_p wb.wb_supervisor_field_get_type_name.restype = ctypes.c_char_p - wb.wb_supervisor_node_get_proto_field.restype = ctypes.c_void_p - wb.wb_supervisor_node_get_proto_field_by_index.restype = ctypes.c_void_p - wb.wb_supervisor_node_get_field.restype = ctypes.c_void_p - wb.wb_supervisor_node_get_field_by_index.restype = ctypes.c_void_p + wb.wb_supervisor_field_get_actual_field.restype = ctypes.c_void_p wb.wb_supervisor_field_get_sf_float.restype = ctypes.c_double wb.wb_supervisor_field_get_sf_vec2f.restype = ctypes.POINTER(ctypes.c_double) wb.wb_supervisor_field_get_sf_vec3f.restype = ctypes.POINTER(ctypes.c_double) @@ -65,17 +62,8 @@ class Field: wb.wb_supervisor_virtual_reality_headset_get_position = ctypes.POINTER(ctypes.c_double) wb.wb_supervisor_virtual_reality_headset_get_orientation = ctypes.POINTER(ctypes.c_double) - def __init__(self, node, name: typing.Optional[str] = None, index: typing.Optional[int] = None, proto: bool = False): - if proto: - if name is not None: - self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_proto_field(node._ref, str.encode(name))) - else: - self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_proto_field_by_index(node._ref, index)) - else: - if name is not None: - self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_field(node._ref, str.encode(name))) - else: - self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_field_by_index(node._ref, index)) + def __init__(self, ref: ctypes.c_void_p): + self._ref = ref if self._ref: self.type = wb.wb_supervisor_field_get_type(self._ref) else: @@ -93,6 +81,10 @@ def getTypeName(self) -> str: def getCount(self) -> int: return self.count + def getActualField(self) -> 'Field': + field = wb.wb_supervisor_field_get_actual_field(self._ref) + return Field(field) if field else None + def enableSFTracking(self, samplingPeriod: int): wb.wb_supervisor_field_enable_sf_tracking(self._ref, samplingPeriod) diff --git a/lib/controller/python/controller/node.py b/lib/controller/python/controller/node.py index a7c92ccf17e..f4747081507 100644 --- a/lib/controller/python/controller/node.py +++ b/lib/controller/python/controller/node.py @@ -16,6 +16,7 @@ from .wb import wb from .constants import constant from .field import Field +from .proto import Proto import struct import typing @@ -40,6 +41,11 @@ class Node: wb.wb_supervisor_node_get_root.restype = ctypes.c_void_p wb.wb_supervisor_node_get_selected.restype = ctypes.c_void_p wb.wb_supervisor_node_get_from_def.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_proto.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_field.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_field_by_index.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_base_node_field.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_base_node_field_by_index.restype = ctypes.c_void_p wb.wb_supervisor_node_get_self.restype = ctypes.c_void_p wb.wb_supervisor_node_get_from_device.restype = ctypes.c_void_p wb.wb_supervisor_node_get_from_id.restype = ctypes.c_void_p @@ -90,6 +96,10 @@ def getParentNode(self) -> Node: def isProto(self) -> bool: return wb.wb_supervisor_node_is_proto(self._ref) != 0 + def getProto(self) -> Proto: + proto = wb.wb_supervisor_node_get_proto(self._ref) + return Proto(proto) if proto else None + def getFromProtoDef(self, DEF: str) -> Node: node = wb.wb_supervisor_node_get_from_proto_def(self._ref, str.encode(DEF)) return Node(ref=node) if node else None @@ -109,24 +119,27 @@ def remove(self): def exportString(self): return wb.wb_supervisor_node_export_string(self._ref).decode() - def getField(self, name: str) -> Field: - field = Field(self, name=name) - return field if field._ref else None + def getField(self, fieldName: str) -> Field: + field = wb.wb_supervisor_node_get_field(self._ref, str.encode(fieldName)) + return Field(field) if field else None def getFieldByIndex(self, index: int) -> Field: - field = Field(self, index=index) - return field if field._ref else None + field = wb.wb_supervisor_node_get_field_by_index(self._ref, index) + return Field(field) if field else None def getNumberOfFields(self) -> int: return self.number_of_fields - def getProtoField(self, name: str) -> Field: - field = Field(self, name=name, proto=True) - return field if field._ref else None + def getBaseNodeField(self, fieldName: str) -> Field: + field = wb.wb_supervisor_node_get_base_node_field(self._ref, str.encode(fieldName)) + return Field(field) if field else None + + def getBaseNodeFieldByIndex(self, index: int) -> Field: + field = wb.wb_supervisor_node_get_base_node_field_by_index(self._ref, index) + return Field(field) if field else None - def getProtoFieldByIndex(self, index: int) -> Field: - field = Field(self, index=index, proto=True) - return field if field._ref else None + def getNumberOfBaseNodeFields(self) -> int: + return self.number_of_base_node_fields def getPosition(self) -> typing.List[float]: p = wb.wb_supervisor_node_get_position(self._ref) @@ -235,6 +248,10 @@ def base_type_name(self) -> str: def number_of_fields(self) -> int: return wb.wb_supervisor_node_get_number_of_fields(self._ref) + @property + def number_of_base_node_fields(self) -> int: + return wb.wb_supervisor_node_get_number_of_base_node_fields(self._ref) + Node.NO_NODE = constant('NODE_NO_NODE') Node.APPEARANCE = constant('NODE_APPEARANCE') diff --git a/lib/controller/python/controller/proto.py b/lib/controller/python/controller/proto.py new file mode 100644 index 00000000000..d1a8a245b0e --- /dev/null +++ b/lib/controller/python/controller/proto.py @@ -0,0 +1,60 @@ +# Copyright 1996-2023 Cyberbotics Ltd. +# +# 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 +# +# https://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 ctypes +from .wb import wb +from .field import Field + + +class Proto: + wb.wb_supervisor_proto_get_type_name.restype = ctypes.c_char_p + wb.wb_supervisor_proto_get_parent.restype = ctypes.c_void_p + wb.wb_supervisor_proto_get_field.restype = ctypes.c_void_p + wb.wb_supervisor_proto_get_field_by_index.restype = ctypes.c_void_p + + def __init__(self, ref: ctypes.c_void_p): + self._ref = ref + + def getTypeName(self) -> str: + return self.type_name + + def getParent(self) -> 'Proto': + proto = wb.wb_supervisor_proto_get_parent(self._ref) + return Proto(proto) if proto else None + + def getField(self, name: str) -> Field: + field = wb.wb_supervisor_proto_get_field(self._ref, str.encode(name)) + return Field(field) if field else None + + def getFieldByIndex(self, index: int) -> Field: + field = wb.wb_supervisor_proto_get_field_by_index(self._ref, index) + return Field(field) if field else None + + def getNumberOfFields(self) -> int: + return self.number_of_fields + + def isDerived(self) -> bool: + return self.is_derived + + @property + def type_name(self) -> str: + return wb.wb_supervisor_proto_get_type_name(self._ref).decode() + + @property + def number_of_fields(self) -> int: + return wb.wb_supervisor_proto_get_number_of_fields(self._ref) + + @property + def is_derived(self) -> bool: + return wb.wb_supervisor_proto_is_derived(self._ref) != 0 diff --git a/projects/default/controllers/ros/RosSupervisor.cpp b/projects/default/controllers/ros/RosSupervisor.cpp index c1ae4f1de31..5b718e512bd 100644 --- a/projects/default/controllers/ros/RosSupervisor.cpp +++ b/projects/default/controllers/ros/RosSupervisor.cpp @@ -88,6 +88,8 @@ RosSupervisor::RosSupervisor(Ros *ros, Supervisor *supervisor) { mRos->nodeHandle()->advertiseService("supervisor/node/get_parent_node", &RosSupervisor::nodeGetParentNodeCallback, this); mNodeIsProtoServer = mRos->nodeHandle()->advertiseService("supervisor/node/is_proto", &RosSupervisor::nodeIsProtoCallback, this); + mNodeGetProtoServer = + mRos->nodeHandle()->advertiseService("supervisor/node/get_proto", &RosSupervisor::nodeGetProtoCallback, this); mNodeGetPositionServer = mRos->nodeHandle()->advertiseService("supervisor/node/get_position", &RosSupervisor::nodeGetPositionCallback, this); mNodeGetOrientationServer = @@ -144,6 +146,8 @@ RosSupervisor::RosSupervisor(Ros *ros, Supervisor *supervisor) { mRos->nodeHandle()->advertiseService("supervisor/field/get_type_name", &RosSupervisor::fieldGetTypeNameCallback, this); mFieldGetCountServer = mRos->nodeHandle()->advertiseService("supervisor/field/get_count", &RosSupervisor::fieldGetCountCallback, this); + mFieldGetActualFieldServer = mRos->nodeHandle()->advertiseService("supervisor/field/get_actual_field", + &RosSupervisor::fieldGetActualFieldCallback, this); mFieldGetBoolServer = mRos->nodeHandle()->advertiseService("supervisor/field/get_bool", &RosSupervisor::fieldGetBoolCallback, this); mFieldGetInt32Server = @@ -205,6 +209,19 @@ RosSupervisor::RosSupervisor(Ros *ros, Supervisor *supervisor) { &RosSupervisor::fieldEnableSFTrackingCallback, this); mFieldDisableSFTrackingServer = mRos->nodeHandle()->advertiseService("supervisor/field/disable_sf_tracking", &RosSupervisor::fieldDisableSFTrackingCallback, this); + + mProtoGetFieldServer = + mRos->nodeHandle()->advertiseService("supervisor/proto/get_field", &RosSupervisor::protoGetFieldCallback, this); + mProtoGetFieldByIndexServer = mRos->nodeHandle()->advertiseService("supervisor/proto/get_field_by_index", + &RosSupervisor::protoGetFieldByIndexCallback, this); + mProtoGetNumberOfFieldsServer = mRos->nodeHandle()->advertiseService("supervisor/proto/get_number_of_fields", + &RosSupervisor::protoGetNumberOfFieldsCallback, this); + mProtoGetParentServer = + mRos->nodeHandle()->advertiseService("supervisor/proto/get_parent", &RosSupervisor::protoGetParentCallback, this); + mProtoGetTypeNameServer = + mRos->nodeHandle()->advertiseService("supervisor/proto/get_type_name", &RosSupervisor::protoGetTypeNameCallback, this); + mProtoIsDerivedServer = + mRos->nodeHandle()->advertiseService("supervisor/proto/is_derived", &RosSupervisor::protoIsDerivedCallback, this); } RosSupervisor::~RosSupervisor() { @@ -563,6 +580,15 @@ bool RosSupervisor::nodeIsProtoCallback(webots_ros::node_is_proto::Request &req, return true; } +bool RosSupervisor::nodeGetProtoCallback(webots_ros::node_get_proto::Request &req, webots_ros::node_get_proto::Response &res) { + assert(this); + if (!req.node) + return false; + Node *node = reinterpret_cast(req.node); + res.proto = reinterpret_cast(node->getProto()); + return true; +} + // cppcheck-suppress constParameter bool RosSupervisor::nodeGetPositionCallback(webots_ros::node_get_position::Request &req, webots_ros::node_get_position::Response &res) { @@ -767,8 +793,8 @@ bool RosSupervisor::nodeGetFieldCallback(webots_ros::node_get_field::Request &re if (!req.node) return false; Node *node = reinterpret_cast(req.node); - if (req.proto) - res.field = reinterpret_cast(node->getProtoField(req.fieldName)); + if (req.queryBaseNode) + res.field = reinterpret_cast(node->getBaseNodeField(req.fieldName)); else res.field = reinterpret_cast(node->getField(req.fieldName)); return true; @@ -780,8 +806,8 @@ bool RosSupervisor::nodeGetFieldByIndexCallback(webots_ros::node_get_field_by_in if (!req.node) return false; Node *node = reinterpret_cast(req.node); - if (req.proto) - res.field = reinterpret_cast(node->getProtoFieldByIndex(req.index)); + if (req.queryBaseNode) + res.field = reinterpret_cast(node->getBaseNodeFieldByIndex(req.index)); else res.field = reinterpret_cast(node->getFieldByIndex(req.index)); return true; @@ -793,7 +819,7 @@ bool RosSupervisor::nodeGetNumberOfFieldsCallback(webots_ros::node_get_number_of if (!req.node) return false; Node *node = reinterpret_cast(req.node); - res.value = req.proto ? node->getProtoNumberOfFields() : node->getNumberOfFields(); + res.value = req.queryBaseNode ? node->getNumberOfBaseNodeFields() : node->getNumberOfFields(); return true; } @@ -974,6 +1000,16 @@ bool RosSupervisor::fieldGetCountCallback(webots_ros::field_get_count::Request & return true; } +bool RosSupervisor::fieldGetActualFieldCallback(webots_ros::field_get_actual_field::Request &req, + webots_ros::field_get_actual_field::Response &res) { + assert(this); + if (!req.field) + return false; + Field *field = reinterpret_cast(req.field); + res.field = reinterpret_cast(field->getActualField()); + return true; +} + bool RosSupervisor::fieldGetBoolCallback(webots_ros::field_get_bool::Request &req, webots_ros::field_get_bool::Response &res) { assert(this); if (!req.field) @@ -1419,3 +1455,63 @@ bool RosSupervisor::fieldDisableSFTrackingCallback(webots_ros::field_disable_sf_ return true; } + +bool RosSupervisor::protoGetFieldCallback(webots_ros::proto_get_field::Request &req, + webots_ros::proto_get_field::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.field = reinterpret_cast(proto->getField(req.fieldName)); + return true; +} + +bool RosSupervisor::protoGetFieldByIndexCallback(webots_ros::proto_get_field_by_index::Request &req, + webots_ros::proto_get_field_by_index::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.field = reinterpret_cast(proto->getFieldByIndex(req.index)); + return true; +} + +bool RosSupervisor::protoGetNumberOfFieldsCallback(webots_ros::proto_get_number_of_fields::Request &req, + webots_ros::proto_get_number_of_fields::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.value = proto->getNumberOfFields(); + return true; +} + +bool RosSupervisor::protoGetParentCallback(webots_ros::proto_get_parent::Request &req, + webots_ros::proto_get_parent::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.proto = reinterpret_cast(proto->getParent()); + return true; +} + +bool RosSupervisor::protoGetTypeNameCallback(webots_ros::proto_get_type_name::Request &req, + webots_ros::proto_get_type_name::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.value = proto->getTypeName(); + return true; +} + +bool RosSupervisor::protoIsDerivedCallback(webots_ros::proto_is_derived::Request &req, + webots_ros::proto_is_derived::Response &res) { + assert(this); + if (!req.proto) + return false; + Proto *proto = reinterpret_cast(req.proto); + res.value = proto->isDerived(); + return true; +} diff --git a/projects/default/controllers/ros/RosSupervisor.hpp b/projects/default/controllers/ros/RosSupervisor.hpp index c41ae76ce2f..9560cb93a6a 100644 --- a/projects/default/controllers/ros/RosSupervisor.hpp +++ b/projects/default/controllers/ros/RosSupervisor.hpp @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -67,6 +68,7 @@ #include #include +#include #include #include #include @@ -91,6 +93,13 @@ #include #include +#include +#include +#include +#include +#include +#include + using namespace webots; class RosSupervisor { @@ -140,6 +149,7 @@ class RosSupervisor { bool nodeGetParentNodeCallback(webots_ros::node_get_parent_node::Request &req, webots_ros::node_get_parent_node::Response &res); bool nodeIsProtoCallback(webots_ros::node_is_proto::Request &req, webots_ros::node_is_proto::Response &res); + bool nodeGetProtoCallback(webots_ros::node_get_proto::Request &req, webots_ros::node_get_proto::Response &res); bool nodeGetPositionCallback(webots_ros::node_get_position::Request &req, webots_ros::node_get_position::Response &res); bool nodeGetOrientationCallback(webots_ros::node_get_orientation::Request &req, webots_ros::node_get_orientation::Response &res); @@ -188,6 +198,8 @@ class RosSupervisor { bool fieldGetTypeCallback(webots_ros::field_get_type::Request &req, webots_ros::field_get_type::Response &res); bool fieldGetTypeNameCallback(webots_ros::field_get_name::Request &req, webots_ros::field_get_name::Response &res); bool fieldGetCountCallback(webots_ros::field_get_count::Request &req, webots_ros::field_get_count::Response &res); + bool fieldGetActualFieldCallback(webots_ros::field_get_actual_field::Request &req, + webots_ros::field_get_actual_field::Response &res); bool fieldGetBoolCallback(webots_ros::field_get_bool::Request &req, webots_ros::field_get_bool::Response &res); bool fieldGetInt32Callback(webots_ros::field_get_int32::Request &req, webots_ros::field_get_int32::Response &res); bool fieldGetFloatCallback(webots_ros::field_get_float::Request &req, webots_ros::field_get_float::Response &res); @@ -222,6 +234,15 @@ class RosSupervisor { bool fieldDisableSFTrackingCallback(webots_ros::field_disable_sf_tracking::Request &req, webots_ros::field_disable_sf_tracking::Response &res); + bool protoGetFieldCallback(webots_ros::proto_get_field::Request &req, webots_ros::proto_get_field::Response &res); + bool protoGetFieldByIndexCallback(webots_ros::proto_get_field_by_index::Request &req, + webots_ros::proto_get_field_by_index::Response &res); + bool protoGetNumberOfFieldsCallback(webots_ros::proto_get_number_of_fields::Request &req, + webots_ros::proto_get_number_of_fields::Response &res); + bool protoGetParentCallback(webots_ros::proto_get_parent::Request &req, webots_ros::proto_get_parent::Response &res); + bool protoGetTypeNameCallback(webots_ros::proto_get_type_name::Request &req, webots_ros::proto_get_type_name::Response &res); + bool protoIsDerivedCallback(webots_ros::proto_is_derived::Request &req, webots_ros::proto_is_derived::Response &res); + private: Supervisor *mSupervisor; Ros *mRos; @@ -259,6 +280,7 @@ class RosSupervisor { ros::ServiceServer mNodeGetBaseTypeNameServer; ros::ServiceServer mNodeGetParentNodeServer; ros::ServiceServer mNodeIsProtoServer; + ros::ServiceServer mNodeGetProtoServer; ros::ServiceServer mNodeGetPositionServer; ros::ServiceServer mNodeGetOrientationServer; ros::ServiceServer mNodeGetPoseServer; @@ -291,6 +313,7 @@ class RosSupervisor { ros::ServiceServer mFieldGetTypeServer; ros::ServiceServer mFieldGetTypeNameServer; ros::ServiceServer mFieldGetCountServer; + ros::ServiceServer mFieldGetActualFieldServer; ros::ServiceServer mFieldGetBoolServer; ros::ServiceServer mFieldGetInt32Server; ros::ServiceServer mFieldGetFloatServer; @@ -321,6 +344,13 @@ class RosSupervisor { ros::ServiceServer mFieldRemoveNodeServer; ros::ServiceServer mFieldEnableSFTrackingServer; ros::ServiceServer mFieldDisableSFTrackingServer; + + ros::ServiceServer mProtoGetFieldServer; + ros::ServiceServer mProtoGetFieldByIndexServer; + ros::ServiceServer mProtoGetNumberOfFieldsServer; + ros::ServiceServer mProtoGetParentServer; + ros::ServiceServer mProtoGetTypeNameServer; + ros::ServiceServer mProtoIsDerivedServer; }; #endif // ROS_SUPERVISOR_HPP diff --git a/resources/webots_ros b/resources/webots_ros index 43fadff485c..9cb0aa22f01 160000 --- a/resources/webots_ros +++ b/resources/webots_ros @@ -1 +1 @@ -Subproject commit 43fadff485c8f62c47e4eb2331d1fb76adab2a24 +Subproject commit 9cb0aa22f0134acb155979d45bea713134dcc426 diff --git a/src/controller/c/Controller.def b/src/controller/c/Controller.def index 4d895a30aa2..d7fc80bbe2b 100644 --- a/src/controller/c/Controller.def +++ b/src/controller/c/Controller.def @@ -462,6 +462,7 @@ wb_supervisor_animation_stop_recording wb_supervisor_export_image wb_supervisor_field_disable_sf_tracking wb_supervisor_field_enable_sf_tracking +wb_supervisor_field_get_actual_field wb_supervisor_field_get_count wb_supervisor_field_get_name wb_supervisor_field_get_mf_bool @@ -525,6 +526,8 @@ wb_supervisor_node_enable_contact_point_tracking wb_supervisor_node_enable_contact_points_tracking wb_supervisor_node_enable_pose_tracking wb_supervisor_node_export_string +wb_supervisor_node_get_base_node_field +wb_supervisor_node_get_base_node_field_by_index wb_supervisor_node_get_base_type_name wb_supervisor_node_get_def wb_supervisor_node_get_field @@ -538,15 +541,14 @@ wb_supervisor_node_get_contact_point wb_supervisor_node_get_contact_points wb_supervisor_node_get_contact_point_node wb_supervisor_node_get_id -wb_supervisor_node_get_number_of_fields +wb_supervisor_node_get_number_of_base_node_fields wb_supervisor_node_get_number_of_contact_points +wb_supervisor_node_get_number_of_fields wb_supervisor_node_get_orientation wb_supervisor_node_get_parent_node wb_supervisor_node_get_pose wb_supervisor_node_get_position -wb_supervisor_node_get_proto_field -wb_supervisor_node_get_proto_field_by_index -wb_supervisor_node_get_proto_number_of_fields +wb_supervisor_node_get_proto wb_supervisor_node_get_root wb_supervisor_node_get_selected wb_supervisor_node_get_self @@ -564,6 +566,12 @@ wb_supervisor_node_save_state wb_supervisor_node_set_joint_position wb_supervisor_node_set_velocity wb_supervisor_node_set_visibility +wb_supervisor_proto_get_field +wb_supervisor_proto_get_field_by_index +wb_supervisor_proto_get_number_of_fields +wb_supervisor_proto_get_parent +wb_supervisor_proto_get_type_name +wb_supervisor_proto_is_derived wb_supervisor_set_label wb_supervisor_simulation_get_mode wb_supervisor_simulation_physics_reset diff --git a/src/controller/c/messages.h b/src/controller/c/messages.h index dad5d67a333..71eb3630e21 100644 --- a/src/controller/c/messages.h +++ b/src/controller/c/messages.h @@ -98,6 +98,7 @@ #define C_SUPERVISOR_NODE_RESET_STATE 75 #define C_SUPERVISOR_NODE_SET_JOINT_POSITION 76 #define C_SUPERVISOR_NODE_EXPORT_STRING 77 +#define C_SUPERVISOR_NODE_GET_PROTO 78 // ctr <-> sim #define C_ROBOT_WAIT_FOR_USER_INPUT_EVENT 80 diff --git a/src/controller/c/supervisor.c b/src/controller/c/supervisor.c index e7a6dc47a19..0dfb5d4d543 100644 --- a/src/controller/c/supervisor.c +++ b/src/controller/c/supervisor.c @@ -55,12 +55,18 @@ union WbFieldData { typedef struct WbFieldStructPrivate { const char *name; - WbFieldType type; // WB_SF_* or WB_MT_* as defined in supervisor.h + WbFieldType type; // WB_SF_* or WB_MF_* as defined in supervisor.h int count; // used in MF fields only int node_unique_id; - int id; // attributed by Webots + int id; // attributed by Webots + int proto_id; bool is_proto_internal_field; // TRUE if this is a PROTO field, FALSE in case of PROTO parameter or NODE field bool is_read_only; // only fields visible from the scene tree can be modified from the Supervisor API + int actual_field_node_id; // the node and field id of the corresponding field in the scene tree (if it exists) + int actual_field_index; + // the lookup_field field will only be populated when we want to override the default value lookup behavior + // (for internal proto fields) + WbFieldRef lookup_field; union WbFieldData data; WbFieldRef next; double last_update; @@ -114,6 +120,7 @@ typedef struct WbNodeStructPrivate { double *solid_velocity; // double[6] (linear[3] + angular[3]) bool is_proto; bool is_proto_internal; // FALSE if the node is visible in the scene tree, otherwise TRUE + WbProtoRef proto_info; WbNodeRef parent_proto; int tag; WbNodeRef next; @@ -121,6 +128,18 @@ typedef struct WbNodeStructPrivate { static WbNodeStruct *node_list = NULL; +typedef struct WbProtoInfoStructPrivate { + const char *type_name; + bool is_derived; + int node_unique_id; + int id; + int number_of_fields; + WbProtoRef parent; + WbProtoRef next; +} WbProtoInfoStruct; + +static WbProtoInfoStruct *proto_list = NULL; + typedef struct WbFieldChangeTrackingPrivate { WbFieldStruct *field; int sampling_period; @@ -175,11 +194,11 @@ static char *supervisor_strdup(const char *src) { } // find field in field_list -static WbFieldStruct *find_field_by_name(const char *field_name, int node_id, bool is_proto_internal_field) { +static WbFieldStruct *find_field_by_name(const char *field_name, int node_id, int proto_id, bool is_proto_internal_field) { // TODO: Hash map needed WbFieldStruct *field = field_list; while (field) { - if (field->node_unique_id == node_id && strcmp(field_name, field->name) == 0 && + if (field->node_unique_id == node_id && strcmp(field_name, field->name) == 0 && field->proto_id == proto_id && field->is_proto_internal_field == is_proto_internal_field) return field; field = field->next; @@ -187,11 +206,12 @@ static WbFieldStruct *find_field_by_name(const char *field_name, int node_id, bo return NULL; } -static WbFieldStruct *find_field_by_id(int node_id, int field_id, bool is_proto_internal_field) { +static WbFieldStruct *find_field_by_id(int node_id, int proto_id, int field_id, bool is_proto_internal_field) { // TODO: Hash map needed WbFieldStruct *field = field_list; while (field) { - if (field->node_unique_id == node_id && field->id == field_id && field->is_proto_internal_field == is_proto_internal_field) + if (field->node_unique_id == node_id && field->proto_id == proto_id && field->id == field_id && + field->is_proto_internal_field == is_proto_internal_field) return field; field = field->next; } @@ -243,6 +263,19 @@ static bool is_node_ref_valid(const WbNodeRef n) { return false; } +static bool is_proto_ref_valid(const WbProtoRef p) { + if (!p) + return false; + + WbProtoRef proto = proto_list; + while (proto) { + if (proto == p) + return true; + proto = proto->next; + } + return false; +} + static void delete_node(WbNodeRef node) { // clean the node free(node->model_name); @@ -257,6 +290,39 @@ static void delete_node(WbNodeRef node) { free(node); } +static void delete_proto(WbProtoInfoStruct *proto) { + free((char *)proto->type_name); + free(proto); +} + +static void delete_field(WbFieldStruct *field) { + if (field->type == WB_SF_STRING || field->type == WB_MF_STRING) + free(field->data.sf_string); + free((char *)field->name); + free(field); +} + +static void remove_proto_from_list(WbProtoRef proto) { + if (!proto) + return; + + // look for the previous proto in the list + if (proto_list == proto) // the proto is the first of the list + proto_list = proto->next; + else { + WbProtoRef previous_proto_in_list = proto_list; + while (previous_proto_in_list) { + if (previous_proto_in_list->next && previous_proto_in_list->next == proto) { + // connect previous and next node in the list + previous_proto_in_list->next = proto->next; + break; + } + previous_proto_in_list = previous_proto_in_list->next; + } + } + delete_proto(proto); +} + static void remove_node_from_list(int uid) { WbNodeRef node = find_node_by_id(uid); if (node) { // make sure this node is in the list @@ -283,6 +349,14 @@ static void remove_node_from_list(int uid) { n->parent_id = -1; n = n->next; } + + WbProtoRef p = proto_list; + while (p) { + WbProtoRef proto = p; + p = p->next; + if (proto->node_unique_id == uid) + remove_proto_from_list(proto); + } } // extract node DEF name from dot expression @@ -333,16 +407,18 @@ static void remove_internal_proto_nodes_and_fields_from_list() { field_list = field->next; WbFieldStruct *current_field = field; field = field->next; - // clean the field - if (current_field->type == WB_SF_STRING || current_field->type == WB_MF_STRING) - free(current_field->data.sf_string); - free((char *)current_field->name); - free(current_field); + delete_field(current_field); } else { previous_field = field; field = field->next; } } + + while (proto_list) { + WbProtoInfoStruct *p = proto_list->next; + delete_proto(proto_list); + proto_list = p; + } } static void add_node_to_list(int uid, WbNodeType type, const char *model_name, const char *def_name, int tag, int parent_id, @@ -382,6 +458,7 @@ static void add_node_to_list(int uid, WbNodeType type, const char *model_name, c n->solid_velocity = NULL; n->is_proto = is_proto; n->is_proto_internal = false; + n->proto_info = NULL; n->parent_proto = NULL; n->tag = tag; n->next = node_list; @@ -444,6 +521,7 @@ static int node_number_of_fields = -1; static int requested_field_index = -1; static bool node_get_selected = false; static int node_ref = 0; +static int proto_ref = -1; static WbNodeRef root_ref = NULL; static WbNodeRef self_node_ref = NULL; static WbNodeRef position_node_ref = NULL; @@ -473,6 +551,7 @@ static const double *add_force_offset = NULL; static WbNodeRef set_joint_node_ref = NULL; static double set_joint_position = 0.0; static int set_joint_index = 0; +static bool node_get_proto = false; static bool virtual_reality_headset_is_used_request = false; static bool virtual_reality_headset_is_used = false; static bool virtual_reality_headset_position_request = false; @@ -488,10 +567,7 @@ static void supervisor_cleanup(WbDevice *d) { clean_field_request_garbage_collector(); while (field_list) { WbFieldStruct *f = field_list->next; - if (field_list->type == WB_SF_STRING || field_list->type == WB_MF_STRING) - free(field_list->data.sf_string); - free((char *)field_list->name); - free(field_list); + delete_field(field_list); field_list = f; } while (field_requests_list_head) { @@ -518,6 +594,11 @@ static void supervisor_cleanup(WbDevice *d) { delete_node(node_list); node_list = n; } + while (proto_list) { + WbProtoInfoStruct *p = proto_list->next; + delete_proto(proto_list); + proto_list = p; + } free(export_image_filename); free(animation_filename); @@ -567,11 +648,13 @@ static void supervisor_write_request(WbDevice *d, WbRequest *r) { } else if (requested_field_name) { request_write_uchar(r, C_SUPERVISOR_FIELD_GET_FROM_NAME); request_write_uint32(r, node_ref); + request_write_int32(r, proto_ref); request_write_string(r, requested_field_name); request_write_uchar(r, allow_search_in_proto ? 1 : 0); } else if (requested_field_index >= 0) { request_write_uchar(r, C_SUPERVISOR_FIELD_GET_FROM_INDEX); request_write_uint32(r, node_ref); + request_write_int32(r, proto_ref); request_write_uint32(r, requested_field_index); request_write_uchar(r, allow_search_in_proto ? 1 : 0); } else if (requested_node_number_of_fields) { @@ -585,6 +668,10 @@ static void supervisor_write_request(WbDevice *d, WbRequest *r) { request_write_uchar(r, pose_change_tracking.enable); if (pose_change_tracking.enable) request_write_int32(r, pose_change_tracking.sampling_period); + } else if (node_get_proto) { + request_write_uchar(r, C_SUPERVISOR_NODE_GET_PROTO); + request_write_uint32(r, node_ref); + request_write_int32(r, proto_ref); } else if (field_change_tracking_requested) { request_write_uchar(r, C_SUPERVISOR_FIELD_CHANGE_TRACKING_STATE); request_write_int32(r, field_change_tracking.field->node_unique_id); @@ -608,6 +695,7 @@ static void supervisor_write_request(WbDevice *d, WbRequest *r) { if (request->type == GET) { request_write_uchar(r, C_SUPERVISOR_FIELD_GET_VALUE); request_write_uint32(r, f->node_unique_id); + request_write_int32(r, f->proto_id); request_write_uint32(r, f->id); request_write_uchar(r, f->is_proto_internal_field ? 1 : 0); if (request->index != -1) @@ -949,12 +1037,32 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { node_id = uid; } } break; + case C_SUPERVISOR_NODE_GET_PROTO: { + const int id = request_read_int32(r); + const bool is_derived = request_read_uchar(r) == 1; + const int num_fields = request_read_int32(r); + const char *type_name = request_read_string(r); + if (id < 0) + break; + WbProtoInfoStruct *p = malloc(sizeof(WbProtoInfoStruct)); + p->type_name = type_name; + p->is_derived = is_derived; + p->node_unique_id = node_ref; + p->id = id; + p->number_of_fields = num_fields; + p->parent = NULL; + + p->next = proto_list; + proto_list = p; + } break; case C_SUPERVISOR_FIELD_GET_FROM_INDEX: case C_SUPERVISOR_FIELD_GET_FROM_NAME: { const int field_ref = request_read_int32(r); const WbFieldType field_type = request_read_int32(r); const bool is_proto_internal_field = request_read_uchar(r) == 1; const int field_count = request_read_int32(r); + const int actual_field_node_id = request_read_int32(r); + const int actual_field_index = request_read_int32(r); const char *name = request_read_string(r); if (field_ref == -1) { requested_field_name = NULL; @@ -967,10 +1075,14 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { f->count = field_count; f->node_unique_id = node_ref; f->name = name; + f->proto_id = proto_ref; f->is_proto_internal_field = is_proto_internal_field; f->is_read_only = is_proto_internal_field; f->last_update = -DBL_MAX; f->data.sf_string = NULL; + f->actual_field_node_id = actual_field_node_id; + f->actual_field_index = actual_field_index; + f->lookup_field = NULL; field_list = f; } break; case C_SUPERVISOR_FIELD_GET_VALUE: { @@ -979,12 +1091,14 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { // field_type == 0 if node was deleted if (field_type != 0) { const int field_node_id = request_read_int32(r); + const int field_proto_id = request_read_int32(r); const int field_id = request_read_int32(r); const bool is_field_get_request = sent_field_get_request && sent_field_get_request->field && sent_field_get_request->field->node_unique_id == field_node_id && + sent_field_get_request->field->proto_id == field_proto_id && sent_field_get_request->field->id == field_id; - WbFieldStruct *f = - (is_field_get_request) ? sent_field_get_request->field : find_field_by_id(field_node_id, field_id, false); + WbFieldStruct *f = (is_field_get_request) ? sent_field_get_request->field : + find_field_by_id(field_node_id, field_proto_id, field_id, false); if (f) { switch (f->type) { case WB_SF_BOOL: @@ -1064,10 +1178,12 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { const int parent_node_id = request_read_int32(r); const char *field_name = request_read_string(r); const int field_count = request_read_int32(r); + // Proto fields do not receive count change events + // They will just defer to the lookup_field if (parent_node_id >= 0) { - WbFieldStruct *field = find_field_by_name(field_name, parent_node_id, false); + WbFieldStruct *field = find_field_by_name(field_name, parent_node_id, -1, false); if (field == NULL) - field = find_field_by_name(field_name, parent_node_id, true); + field = find_field_by_name(field_name, parent_node_id, -1, true); if (field) field->count = field_count; } @@ -2310,18 +2426,19 @@ WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, int index) { robot_mutex_lock(); // search if field is already present in field_list - WbFieldRef result = find_field_by_id(node->id, index, false); + WbFieldRef result = find_field_by_id(node->id, -1, index, false); if (!result) { // otherwise: need to talk to Webots WbFieldRef field_list_before = field_list; requested_field_index = index; node_ref = node->id; + proto_ref = -1; wb_robot_flush_unlocked(__FUNCTION__); requested_field_index = -1; if (field_list != field_list_before) result = field_list; else - result = find_field_by_id(node->id, index, false); + result = find_field_by_id(node->id, -1, index, false); if (result && node->is_proto_internal) result->is_read_only = true; } @@ -2329,7 +2446,7 @@ WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, int index) { return result; } -WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, int index) { +WbFieldRef wb_supervisor_node_get_base_node_field_by_index(WbNodeRef node, int index) { if (!robot_check_supervisor(__FUNCTION__)) return NULL; @@ -2346,19 +2463,20 @@ WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, int index robot_mutex_lock(); // search if field is already present in field_list - WbFieldRef result = find_field_by_id(node->id, index, true); + WbFieldRef result = find_field_by_id(node->id, -1, index, true); if (!result) { // otherwise: need to talk to Webots WbFieldRef field_list_before = field_list; requested_field_index = index; node_ref = node->id; + proto_ref = -1; allow_search_in_proto = true; wb_robot_flush_unlocked(__FUNCTION__); requested_field_index = -1; if (field_list != field_list_before) result = field_list; else - result = find_field_by_id(node->id, index, true); + result = find_field_by_id(node->id, -1, index, true); if (result) result->is_read_only = true; allow_search_in_proto = false; @@ -2384,11 +2502,12 @@ WbFieldRef wb_supervisor_node_get_field(WbNodeRef node, const char *field_name) robot_mutex_lock(); - WbFieldRef result = find_field_by_name(field_name, node->id, false); + WbFieldRef result = find_field_by_name(field_name, node->id, -1, false); if (!result) { // otherwise: need to talk to Webots requested_field_name = field_name; node_ref = node->id; + proto_ref = -1; wb_robot_flush_unlocked(__FUNCTION__); if (requested_field_name) { requested_field_name = NULL; @@ -2423,7 +2542,7 @@ int wb_supervisor_node_get_number_of_fields(WbNodeRef node) { return -1; } -int wb_supervisor_node_get_proto_number_of_fields(WbNodeRef node) { +int wb_supervisor_node_get_number_of_base_node_fields(WbNodeRef node) { if (!robot_check_supervisor(__FUNCTION__)) return -1; @@ -2447,7 +2566,7 @@ int wb_supervisor_node_get_proto_number_of_fields(WbNodeRef node) { return -1; } -WbFieldRef wb_supervisor_node_get_proto_field(WbNodeRef node, const char *field_name) { +WbFieldRef wb_supervisor_node_get_base_node_field(WbNodeRef node, const char *field_name) { if (!robot_check_supervisor(__FUNCTION__)) return NULL; @@ -2471,11 +2590,12 @@ WbFieldRef wb_supervisor_node_get_proto_field(WbNodeRef node, const char *field_ robot_mutex_lock(); // search if field is already present in field_list - WbFieldRef result = find_field_by_name(field_name, node->id, true); + WbFieldRef result = find_field_by_name(field_name, node->id, -1, true); if (!result) { // otherwise: need to talk to Webots requested_field_name = field_name; node_ref = node->id; + proto_ref = -1; allow_search_in_proto = true; wb_robot_flush_unlocked(__FUNCTION__); if (requested_field_name) { @@ -2790,6 +2910,38 @@ void wb_supervisor_node_set_joint_position(WbNodeRef node, double position, int robot_mutex_unlock(); } +WbProtoRef wb_supervisor_node_get_proto(WbNodeRef node) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (!is_node_ref_valid(node)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL 'node' argument.\n", __FUNCTION__); + return NULL; + } + + if (!node->is_proto) + return NULL; + + robot_mutex_lock(); + + if (!is_proto_ref_valid(node->proto_info)) { + // if we don't know the proto info yet, we need to talk to Webots + WbProtoRef proto_list_before = proto_list; + node_ref = node->id; + proto_ref = -1; + node_get_proto = true; + wb_robot_flush_unlocked(__FUNCTION__); + if (proto_list != proto_list_before) + node->proto_info = proto_list; + node_get_proto = false; + } + + robot_mutex_unlock(); + + return node->proto_info; +} + bool wb_supervisor_virtual_reality_headset_is_used() { if (!robot_check_supervisor(__FUNCTION__)) return false; @@ -2845,6 +2997,9 @@ WbFieldType wb_supervisor_field_get_type(WbFieldRef field) { } int wb_supervisor_field_get_count(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false)) return -1; @@ -2854,6 +3009,31 @@ int wb_supervisor_field_get_count(WbFieldRef field) { return ((WbFieldStruct *)field)->count; } +WbFieldRef wb_supervisor_field_get_actual_field(WbFieldRef field) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false)) + return NULL; + + if (!field->is_read_only) + return field; + + if (field->lookup_field) + return field->lookup_field; + + if (field->actual_field_node_id != -1 && field->actual_field_index != -1) { + WbNodeRef node = node_get_from_id(field->actual_field_node_id, __FUNCTION__); + if (node) { + WbFieldRef actual_field = wb_supervisor_node_get_field_by_index(node, field->actual_field_index); + assert(!actual_field || !actual_field->is_read_only); + return actual_field; + } + } + + return NULL; +} + void wb_supervisor_node_enable_contact_points_tracking(WbNodeRef node, int sampling_period, bool include_descendants) { if (sampling_period < 0) { fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); @@ -2925,6 +3105,9 @@ void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool incl } void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_period) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false)) return; @@ -2944,6 +3127,9 @@ void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_perio } void wb_supervisor_field_disable_sf_tracking(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false)) return; @@ -3033,6 +3219,9 @@ void wb_supervisor_node_disable_pose_tracking(WbNodeRef node, WbNodeRef from_nod } bool wb_supervisor_field_get_sf_bool(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_BOOL, true, NULL, false, false)) return false; @@ -3041,6 +3230,9 @@ bool wb_supervisor_field_get_sf_bool(WbFieldRef field) { } int wb_supervisor_field_get_sf_int32(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_INT32, true, NULL, false, false)) return 0; @@ -3049,6 +3241,9 @@ int wb_supervisor_field_get_sf_int32(WbFieldRef field) { } double wb_supervisor_field_get_sf_float(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_FLOAT, true, NULL, false, false)) return 0.0; @@ -3057,6 +3252,9 @@ double wb_supervisor_field_get_sf_float(WbFieldRef field) { } const double *wb_supervisor_field_get_sf_vec2f(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_VEC2F, true, NULL, false, false)) return NULL; @@ -3065,6 +3263,9 @@ const double *wb_supervisor_field_get_sf_vec2f(WbFieldRef field) { } const double *wb_supervisor_field_get_sf_vec3f(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_VEC3F, true, NULL, false, false)) return NULL; @@ -3073,6 +3274,9 @@ const double *wb_supervisor_field_get_sf_vec3f(WbFieldRef field) { } const double *wb_supervisor_field_get_sf_rotation(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_ROTATION, true, NULL, false, false)) return NULL; @@ -3081,6 +3285,9 @@ const double *wb_supervisor_field_get_sf_rotation(WbFieldRef field) { } const double *wb_supervisor_field_get_sf_color(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_COLOR, true, NULL, false, false)) return NULL; @@ -3089,6 +3296,9 @@ const double *wb_supervisor_field_get_sf_color(WbFieldRef field) { } const char *wb_supervisor_field_get_sf_string(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_STRING, true, NULL, false, false)) return ""; @@ -3097,6 +3307,9 @@ const char *wb_supervisor_field_get_sf_string(WbFieldRef field) { } WbNodeRef wb_supervisor_field_get_sf_node(WbFieldRef field) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_NODE, true, NULL, false, false)) return NULL; @@ -3111,6 +3324,9 @@ WbNodeRef wb_supervisor_field_get_sf_node(WbFieldRef field) { } bool wb_supervisor_field_get_mf_bool(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_BOOL, true, &index, false, false)) return 0; @@ -3119,6 +3335,9 @@ bool wb_supervisor_field_get_mf_bool(WbFieldRef field, int index) { } int wb_supervisor_field_get_mf_int32(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_INT32, true, &index, false, false)) return 0; @@ -3127,6 +3346,9 @@ int wb_supervisor_field_get_mf_int32(WbFieldRef field, int index) { } double wb_supervisor_field_get_mf_float(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_FLOAT, true, &index, false, false)) return 0.0; @@ -3135,6 +3357,9 @@ double wb_supervisor_field_get_mf_float(WbFieldRef field, int index) { } const double *wb_supervisor_field_get_mf_vec2f(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_VEC2F, true, &index, false, false)) return NULL; @@ -3143,6 +3368,9 @@ const double *wb_supervisor_field_get_mf_vec2f(WbFieldRef field, int index) { } const double *wb_supervisor_field_get_mf_vec3f(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_VEC3F, true, &index, false, false)) return NULL; @@ -3151,6 +3379,9 @@ const double *wb_supervisor_field_get_mf_vec3f(WbFieldRef field, int index) { } const double *wb_supervisor_field_get_mf_color(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_COLOR, true, &index, false, false)) return NULL; @@ -3159,6 +3390,9 @@ const double *wb_supervisor_field_get_mf_color(WbFieldRef field, int index) { } const double *wb_supervisor_field_get_mf_rotation(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_ROTATION, true, &index, false, false)) return NULL; @@ -3167,6 +3401,9 @@ const double *wb_supervisor_field_get_mf_rotation(WbFieldRef field, int index) { } const char *wb_supervisor_field_get_mf_string(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_STRING, true, &index, false, false)) return ""; @@ -3175,6 +3412,9 @@ const char *wb_supervisor_field_get_mf_string(WbFieldRef field, int index) { } WbNodeRef wb_supervisor_field_get_mf_node(WbFieldRef field, int index) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_MF_NODE, true, &index, false, false)) return NULL; @@ -3186,6 +3426,9 @@ WbNodeRef wb_supervisor_field_get_mf_node(WbFieldRef field, int index) { } void wb_supervisor_field_set_sf_bool(WbFieldRef field, bool value) { + if (check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false) && field->lookup_field) + field = field->lookup_field; + if (!check_field(field, __FUNCTION__, WB_SF_BOOL, true, NULL, false, true)) return; @@ -3675,3 +3918,157 @@ const char *wb_supervisor_field_get_type_name(WbFieldRef field) { return ""; } } + +const char *wb_supervisor_proto_get_type_name(WbProtoRef proto) { + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return ""; + } + + return proto->type_name; +} + +bool wb_supervisor_proto_is_derived(WbProtoRef proto) { + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return false; + } + + return proto->is_derived; +} + +WbProtoRef wb_supervisor_proto_get_parent(WbProtoRef proto) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return NULL; + } + + if (!proto->is_derived) + return NULL; + + robot_mutex_lock(); + + if (!is_proto_ref_valid(proto->parent)) { + // if we don't know the parent yet, we need to talk to Webots + WbProtoRef proto_list_before = proto_list; + node_ref = proto->node_unique_id; + proto_ref = proto->id; + node_get_proto = true; + wb_robot_flush_unlocked(__FUNCTION__); + node_get_proto = false; + if (proto_list != proto_list_before) + proto->parent = proto_list; + } + + robot_mutex_unlock(); + + return proto->parent; +} + +WbFieldRef wb_supervisor_proto_get_field(WbProtoRef proto, const char *field_name) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return NULL; + } + + if (!field_name || !field_name[0]) { + fprintf(stderr, "Error: %s() called with a NULL or empty 'field_name' argument.\n", __FUNCTION__); + return NULL; + } + + robot_mutex_lock(); + + WbFieldRef result = find_field_by_name(field_name, proto->node_unique_id, proto->id, true); + if (!result) { + // otherwise: need to talk to Webots + requested_field_name = field_name; + node_ref = proto->node_unique_id; + proto_ref = proto->id; + wb_robot_flush_unlocked(__FUNCTION__); + if (requested_field_name) { + requested_field_name = NULL; + result = field_list; // was just inserted at list head + if (result) + result->is_read_only = true; + } + } + + robot_mutex_unlock(); + + if (result && result->actual_field_index != -1) { + WbFieldRef actual_field = wb_supervisor_field_get_actual_field(result); + assert(actual_field); + result->lookup_field = actual_field; + } + + return result; +} + +WbFieldRef wb_supervisor_proto_get_field_by_index(WbProtoRef proto, int index) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return NULL; + } + + if (index < 0) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a negative 'index' argument: %d.\n", __FUNCTION__, index); + return NULL; + } else if (index >= proto->number_of_fields) + return NULL; + + robot_mutex_lock(); + // search if field is already present in field_list + WbFieldRef result = find_field_by_id(proto->node_unique_id, proto->id, index, true); + if (!result) { + // otherwise: need to talk to Webots + WbFieldRef field_list_before = field_list; + requested_field_index = index; + node_ref = proto->node_unique_id; + proto_ref = proto->id; + allow_search_in_proto = true; + wb_robot_flush_unlocked(__FUNCTION__); + requested_field_index = -1; + if (field_list != field_list_before) + result = field_list; + else + result = find_field_by_id(proto->node_unique_id, proto->id, index, true); + if (result) + result->is_read_only = true; + allow_search_in_proto = false; + } + + robot_mutex_unlock(); + + if (result && result->actual_field_index != -1) { + WbFieldRef actual_field = wb_supervisor_field_get_actual_field(result); + assert(actual_field); + result->lookup_field = actual_field; + } + + return result; +} + +int wb_supervisor_proto_get_number_of_fields(WbProtoRef proto) { + if (!is_proto_ref_valid(proto)) { + if (!robot_is_quitting()) + fprintf(stderr, "Error: %s() called with a NULL or invalid 'proto' argument.\n", __FUNCTION__); + return -1; + } + + return proto->number_of_fields; +} diff --git a/src/controller/cpp/Field.cpp b/src/controller/cpp/Field.cpp index 529cfdb4676..f96576691f4 100644 --- a/src/controller/cpp/Field.cpp +++ b/src/controller/cpp/Field.cpp @@ -69,6 +69,11 @@ std::string Field::getName() const { return string(wb_supervisor_field_get_name(fieldRef)); } +Field *Field::getActualField() const { + WbFieldRef actualFieldRef = wb_supervisor_field_get_actual_field(fieldRef); + return Field::findField(actualFieldRef); +} + int Field::getCount() const { return wb_supervisor_field_get_count(fieldRef); } diff --git a/src/controller/cpp/Makefile b/src/controller/cpp/Makefile index 89ccd7da9a2..496fcc0a7b2 100644 --- a/src/controller/cpp/Makefile +++ b/src/controller/cpp/Makefile @@ -66,6 +66,7 @@ CPPSRC = Accelerometer.cpp \ Node.cpp \ Pen.cpp \ PositionSensor.cpp \ + Proto.cpp \ Radar.cpp \ RangeFinder.cpp \ Receiver.cpp \ diff --git a/src/controller/cpp/Node.cpp b/src/controller/cpp/Node.cpp index c8a3cce7288..613b108d69f 100644 --- a/src/controller/cpp/Node.cpp +++ b/src/controller/cpp/Node.cpp @@ -87,12 +87,17 @@ bool Node::isProto() const { return wb_supervisor_node_is_proto(nodeRef); } +Proto *Node::getProto() const { + WbProtoRef protoRef = wb_supervisor_node_get_proto(nodeRef); + return Proto::findProto(protoRef); +} + int Node::getNumberOfFields() const { return wb_supervisor_node_get_number_of_fields(nodeRef); } -int Node::getProtoNumberOfFields() const { - return wb_supervisor_node_get_proto_number_of_fields(nodeRef); +int Node::getNumberOfBaseNodeFields() const { + return wb_supervisor_node_get_number_of_base_node_fields(nodeRef); } Field *Node::getField(const std::string &fieldName) const { @@ -105,13 +110,13 @@ Field *Node::getFieldByIndex(const int index) const { return Field::findField(fieldRef); } -Field *Node::getProtoField(const std::string &fieldName) const { - WbFieldRef fieldRef = wb_supervisor_node_get_proto_field(nodeRef, fieldName.c_str()); +Field *Node::getBaseNodeField(const std::string &fieldName) const { + WbFieldRef fieldRef = wb_supervisor_node_get_base_node_field(nodeRef, fieldName.c_str()); return Field::findField(fieldRef); } -Field *Node::getProtoFieldByIndex(const int index) const { - WbFieldRef fieldRef = wb_supervisor_node_get_proto_field_by_index(nodeRef, index); +Field *Node::getBaseNodeFieldByIndex(const int index) const { + WbFieldRef fieldRef = wb_supervisor_node_get_base_node_field_by_index(nodeRef, index); return Field::findField(fieldRef); } diff --git a/src/controller/cpp/Proto.cpp b/src/controller/cpp/Proto.cpp new file mode 100644 index 00000000000..bb722e00ee7 --- /dev/null +++ b/src/controller/cpp/Proto.cpp @@ -0,0 +1,73 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// 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 +// +// https://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. + +#define WB_ALLOW_MIXING_C_AND_CPP_API +#include +#include + +#include +#include + +using namespace std; +using namespace webots; + +static map protoMap; + +Proto *Proto::findProto(WbProtoRef ref) { + if (!ref) + return NULL; + + map::iterator iter = protoMap.find(ref); + if (iter != protoMap.end()) + return iter->second; + + Proto *proto = new Proto(ref); + protoMap.insert(pair(ref, proto)); + return proto; +} + +void Proto::cleanup() { + map::iterator iter; + for (iter = protoMap.begin(); iter != protoMap.end(); ++iter) + delete (*iter).second; + + protoMap.clear(); +} + +Proto::Proto(WbProtoRef ref) : protoRef(ref) { +} + +string Proto::getTypeName() const { + return wb_supervisor_proto_get_type_name(protoRef); +} + +bool Proto::isDerived() const { + return wb_supervisor_proto_is_derived(protoRef); +} + +Proto *Proto::getParent() const { + return findProto(wb_supervisor_proto_get_parent(protoRef)); +} + +Field *Proto::getField(const string &fieldName) const { + return Field::findField(wb_supervisor_proto_get_field(protoRef, fieldName.c_str())); +} + +Field *Proto::getFieldByIndex(const int index) const { + return Field::findField(wb_supervisor_proto_get_field_by_index(protoRef, index)); +} + +int Proto::getNumberOfFields() const { + return wb_supervisor_proto_get_number_of_fields(protoRef); +} diff --git a/src/controller/cpp/Supervisor.cpp b/src/controller/cpp/Supervisor.cpp index f478974df61..106bdc3ec29 100644 --- a/src/controller/cpp/Supervisor.cpp +++ b/src/controller/cpp/Supervisor.cpp @@ -23,6 +23,7 @@ using namespace webots; Supervisor::~Supervisor() { Field::cleanup(); Node::cleanup(); + Proto::cleanup(); } Supervisor *Supervisor::getSupervisorInstance() { diff --git a/src/controller/java/Makefile b/src/controller/java/Makefile index 8f1057defae..e89e798872e 100644 --- a/src/controller/java/Makefile +++ b/src/controller/java/Makefile @@ -70,6 +70,7 @@ JAVA_FILES = Accelerometer.java \ Pen.java \ PositionSensor.java \ Propeller.java \ + Proto.java \ Radar.java \ RadarTarget.java \ RangeFinder.java \ diff --git a/src/controller/java/controller.i b/src/controller/java/controller.i index e625178376a..b72ed0e0ad4 100644 --- a/src/controller/java/controller.i +++ b/src/controller/java/controller.i @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -436,12 +437,20 @@ namespace webots { %ignore webots::Field::findField(WbFieldRef ref); %ignore webots::Field::cleanup(); +%rename("getActualFieldPrivate") getActualField() const; +%javamethodmodifiers getActualField() const "private" + %rename("getSFNodePrivate") getSFNode() const; %rename("getMFNodePrivate") getMFNode(int index) const; %javamethodmodifiers getSFNode() const "private" %javamethodmodifiers getMFNode(int index) const "private" %typemap(javacode) webots::Field %{ + public Field getActualField() { + long cPtr = wrapperJNI.Field_getActualFieldPrivate(swigCPtr, this); + return Field.findField(cPtr); + } + public Node getSFNode() { long cPtr = wrapperJNI.Field_getSFNodePrivate(swigCPtr, this); return Node.findNode(cPtr); @@ -632,11 +641,20 @@ namespace webots { %rename("getParentNodePrivate") getParentNode() const; %javamethodmodifiers getParentNode() const "private" +%rename("getProtoPrivate") getProto() const; +%javamethodmodifiers getProto() const "private" + %rename("getFromProtoDefPrivate") getFromProtoDef(const std::string &name) const; %javamethodmodifiers getFromProtoDef(const std::string &name) const "private" %rename("getFieldPrivate") getField(const std::string &fieldName) const; +%rename("getFieldByIndexPrivate") getFieldByIndex(const int index) const; +%rename("getBaseNodeFieldPrivate") getBaseNodeField(const std::string &fieldName) const; +%rename("getBaseNodeFieldByIndexPrivate") getBaseNodeFieldByIndex(const int index) const; %javamethodmodifiers getField(const std::string &fieldName) const "private" +%javamethodmodifiers getFieldByIndex(const int index) const "private" +%javamethodmodifiers getBaseNodeField(const std::string &fieldName) const "private" +%javamethodmodifiers getBaseNodeFieldByIndex(const int index) const "private" %apply int *OUTPUT { int *size }; %rename(getContactPointsPrivate) getContactPoints; @@ -672,6 +690,11 @@ namespace webots { return Node.findNode(cPtr); } + public Proto getProto() { + long cPtr = wrapperJNI.Node_getProtoPrivate(swigCPtr, this); + return Proto.findProto(cPtr); + } + public Node getFromProtoDef(String name) { long cPtr = wrapperJNI.Node_getFromProtoDefPrivate(swigCPtr, this, name); return Node.findNode(cPtr); @@ -682,6 +705,21 @@ namespace webots { return Field.findField(cPtr); } + public Field getFieldByIndex(int index) { + long cPtr = wrapperJNI.Node_getFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + + public Field getBaseNodeField(String fieldName) { + long cPtr = wrapperJNI.Node_getBaseNodeFieldPrivate(swigCPtr, this, fieldName); + return Field.findField(cPtr); + } + + public Field getBaseNodeFieldByIndex(int index) { + long cPtr = wrapperJNI.Node_getBaseNodeFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + private static java.util.HashMap nodes = new java.util.HashMap(); // DO NOT USE THIS FUNCTION: IT IS RESERVED FOR INTERNAL USE ! @@ -733,6 +771,57 @@ namespace webots { %} %include +//---------------------------------------------------------------------------------------------- +// Proto +//---------------------------------------------------------------------------------------------- + +%ignore webots::Proto::findProto(WbProtoRef ref); +%ignore webots::Proto::cleanup(); + +%rename("getParentPrivate") getParent() const; +%javamethodmodifiers getParent() const "private" + +%rename("getFieldPrivate") getField(const std::string &fieldName) const; +%rename("getFieldByIndexPrivate") getFieldByIndex(const int index) const; +%javamethodmodifiers getField(const std::string &fieldName) const "private" +%javamethodmodifiers getFieldByIndex(const int index) const "private" + +%typemap(javacode) webots::Proto %{ +// ----- begin hand written section ---- + public Proto getParent() { + long cPtr = wrapperJNI.Proto_getParentPrivate(swigCPtr, this); + return Proto.findProto(cPtr); + } + + public Field getField(String fieldName) { + long cPtr = wrapperJNI.Proto_getFieldPrivate(swigCPtr, this, fieldName); + return Field.findField(cPtr); + } + + public Field getFieldByIndex(int index) { + long cPtr = wrapperJNI.Proto_getFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + + private static java.util.HashMap protos = new java.util.HashMap(); + + // DO NOT USE THIS FUNCTION: IT IS RESERVED FOR INTERNAL USE ! + public static Proto findProto(long cPtr) { + if (cPtr == 0) + return null; + + Proto proto = protos.get(new Long(cPtr)); + if (proto != null) + return proto; + + proto = new Proto(cPtr, false); + protos.put(new Long(cPtr), proto); + return proto; + } +%} + +%include + //---------------------------------------------------------------------------------------------- // Radar //---------------------------------------------------------------------------------------------- diff --git a/src/controller/matlab/mgenerate.py b/src/controller/matlab/mgenerate.py index 4d2a94f9a54..3e27f5b3b87 100755 --- a/src/controller/matlab/mgenerate.py +++ b/src/controller/matlab/mgenerate.py @@ -445,6 +445,7 @@ def main(args=None): generator.gen(PROC, "wb_supervisor_export_image(filename, quality)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_disable_sf_tracking(field)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_enable_sf_tracking(field, sampling_period)", "supervisor") + generator.gen(FUNC, "wb_supervisor_field_get_actual_field(fieldref)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_get_count(fieldref)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_get_mf_bool(fieldref, index)", "supervisor") # generator.gen(FUNC, "wb_supervisor_field_get_mf_color(fieldref, index)", "supervisor") @@ -517,6 +518,8 @@ def main(args=None): "supervisor") # DEPRECATED generator.gen(FUNC, "wb_supervisor_node_enable_pose_tracking(sampling_period, node, from_node)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_export_string(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_base_node_field(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_base_node_field_by_index(noderef, fieldindex)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_base_type_name(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_center_of_mass(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_contact_point(noderef, index)", "supervisor") @@ -524,21 +527,20 @@ def main(args=None): # generator.gen(PROC, "wb_supervisor_node_get_contact_points(noderef, include_descendants)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_def(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_field(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_field_by_index(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_field_by_index(noderef, fieldindex)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_def(defname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_device(tag)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_id(id)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_proto_def(noderef, defname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_id(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_number_of_base_node_fields(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_number_of_contact_points(noderef, include_descendants)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_number_of_fields(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_number_of_fields(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_orientation(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_parent_node(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_proto(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_pose(noderef, noderef_from)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_position(noderef)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_field(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_field_by_index(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_number_of_fields(noderef, fieldname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_root()", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_selected()", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_self()", "supervisor") @@ -556,6 +558,12 @@ def main(args=None): generator.gen(FUNC, "wb_supervisor_node_set_joint_position(noderef, position, index)", "supervisor") generator.gen(PROC, "wb_supervisor_node_set_velocity(noderef, velocity)", "supervisor") generator.gen(PROC, "wb_supervisor_node_set_visibility(node, from, visible)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_field(protoref, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_field_by_index(protoref, fieldindex)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_number_of_fields(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_parent(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_type_name(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_is_derived(protoref)", "supervisor") # generator.gen(FUNC, "wb_supervisor_save_world(filename)", "supervisor"); # DEPRECATED # generator.gen(PROC, "wb_supervisor_set_label()", "supervisor") generator.gen(FUNC, "wb_supervisor_simulation_get_mode()", "supervisor") diff --git a/src/webots/Makefile b/src/webots/Makefile index 85050852db0..caf62bac4a4 100644 --- a/src/webots/Makefile +++ b/src/webots/Makefile @@ -524,6 +524,7 @@ OTHER_SOURCES = \ WbNetwork.cpp \ WbNodeFactory.cpp \ WbNodeModel.cpp \ + WbNodeProtoInfo.cpp \ WbNodeUtilities.cpp \ WbObjectDetection.cpp \ WbOdeDebugger.cpp \ diff --git a/src/webots/core/WbLanguage.cpp b/src/webots/core/WbLanguage.cpp index fe08d24c587..56dafc4da3c 100644 --- a/src/webots/core/WbLanguage.cpp +++ b/src/webots/core/WbLanguage.cpp @@ -360,6 +360,7 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "wb_supervisor_export_image " "wb_supervisor_field_disable_sf_tracking " "wb_supervisor_field_enable_sf_tracking " + "wb_supervisor_field_get_actual_field " "wb_supervisor_field_get_name " "wb_supervisor_field_get_type " "wb_supervisor_field_get_type_name " @@ -424,6 +425,8 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "wb_supervisor_node_enable_contact_points_tracking " "wb_supervisor_node_enable_pose_tracking " "wb_supervisor_node_export_string " + "wb_supervisor_node_get_base_node_field " + "wb_supervisor_node_get_base_node_field_by_index " "wb_supervisor_node_get_base_type_name " "wb_supervisor_node_get_center_of_mass " "wb_supervisor_node_get_contact_point " @@ -436,15 +439,14 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "wb_supervisor_node_get_from_proto_def " "wb_supervisor_node_get_from_device " "wb_supervisor_node_get_id " + "wb_supervisor_node_get_number_of_base_node_fields " "wb_supervisor_node_get_number_of_contact_points " "wb_supervisor_node_get_number_of_fields " "wb_supervisor_node_get_orientation " "wb_supervisor_node_get_parent_node " "wb_supervisor_node_get_pose " "wb_supervisor_node_get_position " - "wb_supervisor_node_get_proto_field " - "wb_supervisor_node_get_proto_field_by_index " - "wb_supervisor_node_get_proto_number_of_fields " + "wb_supervisor_node_get_proto " "wb_supervisor_node_get_root " "wb_supervisor_node_get_selected " "wb_supervisor_node_get_self " @@ -462,6 +464,12 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "wb_supervisor_node_set_joint_position " "wb_supervisor_node_set_velocity " "wb_supervisor_node_set_visibility " + "wb_supervisor_proto_get_number_of_parameters " + "wb_supervisor_proto_get_field " + "wb_supervisor_proto_get_field_by_index " + "wb_supervisor_proto_get_parent " + "wb_supervisor_proto_get_type_name " + "wb_supervisor_proto_is_derived " "wb_supervisor_set_label " "wb_supervisor_simulation_get_mode " "wb_supervisor_simulation_quit " @@ -557,6 +565,7 @@ static const char *C_API_DATA_TYPES = "WbCameraRecognitionObject " "WbMouseState " "WbNodeRef " "WbNodeType " + "WbProtoRef " "WbRadarTarget " "WbRobotMode " "WbSimulationMode " @@ -709,7 +718,7 @@ static const char *API_CLASSES = "Accelerometer Altimeter Brake Camera CameraRecognitionObject Car Compass Connector ControlMode " "CoordinateSystem Driver Display DistanceSensor Emitter EngineType Field GPS Gyro Keyboard " "InertialUnit ImageRef IndicatorState Joystick LED Lidar LidarPoint LightSensor LinearMotor Mode Motion Motor " - "MouseState Node Pen PositionSensor Radar RadarTarget RangeFinder Receiver Robot RotationalMotor " + "MouseState Node Pen PositionSensor Proto Radar RadarTarget RangeFinder Receiver Robot RotationalMotor " "SimulationMode Skin Speaker Supervisor TouchSensor Type UserInputEvent VacuumGripper WiperMode WheelIndex"; static const char *WBT_OBJECTS = diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.cpp b/src/webots/nodes/utils/WbSupervisorUtilities.cpp index f929554cd1e..f63061a31cf 100644 --- a/src/webots/nodes/utils/WbSupervisorUtilities.cpp +++ b/src/webots/nodes/utils/WbSupervisorUtilities.cpp @@ -33,6 +33,7 @@ #include "WbMFVector2.hpp" #include "WbMFVector3.hpp" #include "WbNodeOperations.hpp" +#include "WbNodeProtoInfo.hpp" #include "WbNodeUtilities.hpp" #include "WbProject.hpp" #include "WbRgb.hpp" @@ -94,6 +95,7 @@ struct WbFieldGetRequest { WbField *field; int fieldId; int nodeId; + int protoId; int index; // for MF fields only }; @@ -305,11 +307,17 @@ void WbSupervisorUtilities::initControllerRequests() { mFoundNodeIsProto = false; mFoundNodeIsProtoInternal = false; mNodeFieldCount = -1; + mFoundProtoId = -2; + mFoundProtoTypeName.clear(); + mFoundProtoIsDerived = false; + mFoundProtoParameterCount = -1; mFoundFieldIndex = -2; mFoundFieldType = 0; mFoundFieldCount = -1; mFoundFieldIsInternal = false; mFoundFieldName.clear(); + mFoundFieldActualFieldNodeId = -1; + mFoundFieldActualFieldIndex = -1; mGetNodeRequest = 0; mNodeGetPosition = NULL; mNodeGetOrientation = NULL; @@ -1190,10 +1198,37 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { mNodeFieldCount = -1; return; } + case C_SUPERVISOR_NODE_GET_PROTO: { + int nodeId, parentProtoId; + stream >> nodeId; + stream >> parentProtoId; + + mFoundProtoId = -1; + mFoundProtoTypeName = ""; + mFoundProtoIsDerived = false; + mFoundProtoParameterCount = -1; + + const WbNode *const node = WbNode::findNode(nodeId); + if (node && node->isProtoInstance()) { + if (parentProtoId < 0) + mFoundProtoId = 0; + else if (parentProtoId < node->protoParents().size() - 1) + mFoundProtoId = parentProtoId + 1; + else + return; + + const WbNodeProtoInfo *protoInfo = node->protoParents().at(mFoundProtoId); + mFoundProtoTypeName = protoInfo->modelName(); + mFoundProtoIsDerived = node->protoParents().size() > mFoundProtoId + 1; + mFoundProtoParameterCount = protoInfo->parameters().size(); + } + return; + } case C_SUPERVISOR_FIELD_GET_FROM_NAME: { - int id; + int nodeId, protoIndex; unsigned char allowSearchInProto; - stream >> id; + stream >> nodeId; + stream >> protoIndex; const QString name = readString(stream); stream >> allowSearchInProto; @@ -1201,38 +1236,74 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { mFoundFieldType = 0; mFoundFieldCount = -1; mFoundFieldIsInternal = false; + mFoundFieldActualFieldNodeId = -1; + mFoundFieldActualFieldIndex = -1; - const WbNode *const node = WbNode::findNode(id); + const WbNode *const node = WbNode::findNode(nodeId); if (node) { - id = node->findFieldId(name, allowSearchInProto == 1); - if (id != -1) { - const WbField *field = node->field(id, allowSearchInProto == 1); - if (field) { - const WbMultipleValue *mv = dynamic_cast(field->value()); - const WbSFNode *sfNode = dynamic_cast(field->value()); - if (mv) - mFoundFieldCount = mv->size(); - else if (sfNode) - mFoundFieldCount = sfNode->value() ? 1 : 0; - - mFoundFieldIndex = id; - mFoundFieldType = field->type(); - mFoundFieldIsInternal = allowSearchInProto == 1; - mFoundFieldName = field->name(); - if (mv || sfNode) { - mWatchedFields.append(WbUpdatedFieldInfo(node->uniqueId(), field->name(), mFoundFieldCount)); - field->listenToValueSizeChanges(); - connect(field, &WbField::valueSizeChanged, this, &WbSupervisorUtilities::notifyFieldUpdate, Qt::UniqueConnection); + int fieldId; + const WbField *field = NULL; + if (protoIndex < 0) { + fieldId = node->findFieldId(name, allowSearchInProto == 1); + if (fieldId != -1) { + field = node->field(fieldId, allowSearchInProto == 1); + if (field) { + const WbMultipleValue *mv = dynamic_cast(field->value()); + const WbSFNode *sfNode = dynamic_cast(field->value()); + if (mv) + mFoundFieldCount = mv->size(); + else if (sfNode) + mFoundFieldCount = sfNode->value() ? 1 : 0; + + mFoundFieldIsInternal = allowSearchInProto == 1; + mFoundFieldName = field->name(); + + if (mv || sfNode) { + mWatchedFields.append(WbUpdatedFieldInfo(node->uniqueId(), field->name(), mFoundFieldCount)); + field->listenToValueSizeChanges(); + connect(field, &WbField::valueSizeChanged, this, &WbSupervisorUtilities::notifyFieldUpdate, + Qt::UniqueConnection); + } } } + } else if (protoIndex < node->protoParents().size()) { + const WbNodeProtoInfo *protoInfo = node->protoParents().at(protoIndex); + fieldId = protoInfo->findFieldIndex(name); + if (fieldId >= 0) { + const WbFieldReference &fieldRef = protoInfo->findFieldByIndex(fieldId); + field = fieldRef.actualField; + mFoundFieldIsInternal = true; + mFoundFieldName = fieldRef.name; + } + + // the supervisor will lookup the actual field for proto field reads, so there's no need send the count + // or listen to value size changes + } + + if (field) { + mFoundFieldIndex = fieldId; + mFoundFieldType = field->type(); + + if (WbVrmlNodeUtilities::isVisible(field)) { + // This only tells us that there is a corresponding parameter in the scene tree. + // Not that this specific field is the actual field. We still have to find it. + const WbField *actualField = field; + while (actualField->parameter()) + actualField = actualField->parameter(); + + mFoundFieldActualFieldNodeId = actualField->parentNode()->uniqueId(); + mFoundFieldActualFieldIndex = actualField->parentNode()->fieldsOrParameters().indexOf(actualField); + assert(mFoundFieldActualFieldIndex >= 0); + } } } return; } case C_SUPERVISOR_FIELD_GET_FROM_INDEX: { - int nodeId, fieldIndex; + int nodeId, protoIndex, fieldIndex; unsigned char allowSearchInProto; stream >> nodeId; + stream >> protoIndex; stream >> fieldIndex; stream >> allowSearchInProto; @@ -1240,25 +1311,58 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { mFoundFieldType = 0; mFoundFieldCount = -1; mFoundFieldIsInternal = false; + mFoundFieldActualFieldNodeId = -1; + mFoundFieldActualFieldIndex = -1; const WbNode *const node = WbNode::findNode(nodeId); if (node) { - const WbField *field = node->field(fieldIndex, allowSearchInProto == 1); + const WbField *field = NULL; + if (protoIndex < 0) { + field = node->field(fieldIndex, allowSearchInProto == 1); + if (field) { + const WbMultipleValue *mv = dynamic_cast(field->value()); + const WbSFNode *sfNode = dynamic_cast(field->value()); + if (mv) + mFoundFieldCount = mv->size(); + else if (sfNode) + mFoundFieldCount = sfNode->value() ? 1 : 0; + + mFoundFieldIsInternal = allowSearchInProto == 1; + mFoundFieldName = field->name(); + + if (mv || sfNode) { + mWatchedFields.append(WbUpdatedFieldInfo(node->uniqueId(), field->name(), mFoundFieldCount)); + field->listenToValueSizeChanges(); + connect(field, &WbField::valueSizeChanged, this, &WbSupervisorUtilities::notifyFieldUpdate, Qt::UniqueConnection); + } + } + } else if (protoIndex < node->protoParents().size()) { + const WbNodeProtoInfo *protoInfo = node->protoParents().at(protoIndex); + const WbFieldReference &fieldRef = protoInfo->findFieldByIndex(fieldIndex); + field = fieldRef.actualField; + if (field) { + mFoundFieldIsInternal = true; + mFoundFieldName = fieldRef.name; + } + + // the supervisor will lookup the actual field for proto field reads, so there's no need send the count + // or listen to value size changes + } + if (field) { - const WbMultipleValue *mv = dynamic_cast(field->value()); - const WbSFNode *sfNode = dynamic_cast(field->value()); - if (mv) - mFoundFieldCount = mv->size(); - else if (sfNode) - mFoundFieldCount = sfNode->value() ? 1 : 0; mFoundFieldIndex = fieldIndex; mFoundFieldType = field->type(); - mFoundFieldIsInternal = allowSearchInProto == 1; - mFoundFieldName = field->name(); - if (mv || sfNode) { - mWatchedFields.append(WbUpdatedFieldInfo(node->uniqueId(), field->name(), mFoundFieldCount)); - field->listenToValueSizeChanges(); - connect(field, &WbField::valueSizeChanged, this, &WbSupervisorUtilities::notifyFieldUpdate, Qt::UniqueConnection); + + if (WbVrmlNodeUtilities::isVisible(field)) { + // This only tells us that there is a corresponding parameter in the scene tree. + // Not that this specific field is the actual field. We still have to find it. + const WbField *actualField = field; + while (actualField->parameter()) + actualField = actualField->parameter(); + + mFoundFieldActualFieldNodeId = actualField->parentNode()->uniqueId(); + mFoundFieldActualFieldIndex = actualField->parentNode()->fieldsOrParameters().indexOf(actualField); + assert(mFoundFieldActualFieldIndex >= 0); } } } @@ -1447,10 +1551,11 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { } case C_SUPERVISOR_FIELD_GET_VALUE: { unsigned int uniqueId, fieldId; - int index = -1; + int protoId, index = -1; unsigned char internal = false; stream >> uniqueId; + stream >> protoId; stream >> fieldId; stream >> internal; @@ -1458,7 +1563,13 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { WbField *field = NULL; if (node) { - field = node->field(fieldId, internal == 1); + if (protoId < 0) { + field = node->field(fieldId, internal == 1); + } else if (protoId < node->protoParents().size()) { + const WbNodeProtoInfo *protoInfo = node->protoParents().at(protoId); + field = protoInfo->findFieldByIndex(fieldId).actualField; + } + if (field && field->isMultiple()) stream >> index; } @@ -1469,6 +1580,7 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { mFieldGetRequest->index = index; mFieldGetRequest->fieldId = fieldId; mFieldGetRequest->nodeId = uniqueId; + mFieldGetRequest->protoId = protoId; return; } case C_SUPERVISOR_FIELD_SET_VALUE: { @@ -1936,6 +2048,16 @@ void WbSupervisorUtilities::writeAnswer(WbDataStream &stream) { stream.writeRawData(s.constData(), s.size() + 1); mFoundNodeUniqueId = -1; } + if (mFoundProtoId != -2) { // enabled, -1 means not found + stream << (short unsigned int)0; + stream << (unsigned char)C_SUPERVISOR_NODE_GET_PROTO; + stream << (int)mFoundProtoId; + stream << (unsigned char)mFoundProtoIsDerived; + stream << (int)mFoundProtoParameterCount; + const QByteArray ba = mFoundProtoTypeName.toUtf8(); + stream.writeRawData(ba.constData(), ba.size() + 1); + mFoundProtoId = -2; + } if (mFoundFieldIndex != -2) { // enabled, -1 means not found stream << (short unsigned int)0; stream << (unsigned char)C_SUPERVISOR_FIELD_GET_FROM_NAME; @@ -1943,6 +2065,8 @@ void WbSupervisorUtilities::writeAnswer(WbDataStream &stream) { stream << (int)mFoundFieldType; stream << (unsigned char)mFoundFieldIsInternal; stream << (int)mFoundFieldCount; + stream << (int)mFoundFieldActualFieldNodeId; + stream << (int)mFoundFieldActualFieldIndex; const QByteArray ba = mFoundFieldName.toUtf8(); stream.writeRawData(ba.constData(), ba.size() + 1); mFoundFieldIndex = -2; @@ -2058,6 +2182,7 @@ void WbSupervisorUtilities::writeAnswer(WbDataStream &stream) { stream << (unsigned char)C_SUPERVISOR_FIELD_GET_VALUE; stream << (int)field.field->type(); stream << (int)field.nodeId; + stream << (int)-1; // Proto fields cannot be tracked stream << (int)field.fieldId; pushSingleFieldContentToStream(stream, field.field); field.lastUpdate = time; @@ -2076,6 +2201,7 @@ void WbSupervisorUtilities::writeAnswer(WbDataStream &stream) { } stream << (int)field->type(); stream << (int)mFieldGetRequest->nodeId; + stream << (int)mFieldGetRequest->protoId; stream << (int)mFieldGetRequest->fieldId; switch (field->type()) { case WB_MF_BOOL: { diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.hpp b/src/webots/nodes/utils/WbSupervisorUtilities.hpp index e04eccc9e22..eb484b61d0e 100644 --- a/src/webots/nodes/utils/WbSupervisorUtilities.hpp +++ b/src/webots/nodes/utils/WbSupervisorUtilities.hpp @@ -84,11 +84,17 @@ private slots: int mFoundNodeParentUniqueId; bool mFoundNodeIsProto; bool mFoundNodeIsProtoInternal; + int mFoundProtoId; + QString mFoundProtoTypeName; + bool mFoundProtoIsDerived; + int mFoundProtoParameterCount; int mFoundFieldIndex; int mFoundFieldType; int mFoundFieldCount; QString mFoundFieldName; bool mFoundFieldIsInternal; + int mFoundFieldActualFieldNodeId; + int mFoundFieldActualFieldIndex; int mNodeFieldCount; int mGetNodeRequest; QList mUpdatedNodeIds; diff --git a/src/webots/vrml/WbNode.cpp b/src/webots/vrml/WbNode.cpp index 96cb4cce56b..bf47e7704ca 100644 --- a/src/webots/vrml/WbNode.cpp +++ b/src/webots/vrml/WbNode.cpp @@ -29,6 +29,7 @@ #include "WbNetwork.hpp" #include "WbNodeFactory.hpp" #include "WbNodeModel.hpp" +#include "WbNodeProtoInfo.hpp" #include "WbNodeReader.hpp" #include "WbParser.hpp" #include "WbProject.hpp" @@ -200,6 +201,8 @@ WbNode::WbNode(const WbNode &other) : if (other.mProto) { mProto = other.mProto; mProto->ref(); + foreach (const WbNodeProtoInfo *protoInfo, other.mProtoParents) + mProtoParents << new WbNodeProtoInfo(*protoInfo); } // do not redirect fields of DEF node descendant even if included in a PROTO parameter @@ -219,11 +222,27 @@ WbNode::WbNode(const WbNode &other) : WbField *copy = new WbField(*parameter, this); mParameters.append(copy); connect(copy, &WbField::valueChanged, this, &WbNode::notifyParameterChanged); + + // Redirect field references in proto info + foreach (WbNodeProtoInfo *protoInfo, mProtoParents) + protoInfo->redirectFields(parameter, copy); } // connect fields to PROTO parameters foreach (WbField *parameter, mParameters) redirectAliasedFields(parameter, this); + + // copy internal PROTO parameters + foreach (const WbField *parameter, other.mInternalProtoParameters) { + WbField *copy = new WbField(*parameter, this); + mInternalProtoParameters << copy; + + // No need to connect any of these. They can only be changed by regenerating the proto + + // Redirect field references in proto info + foreach (WbNodeProtoInfo *protoInfo, mProtoParents) + protoInfo->redirectFields(parameter, copy); + } } gParent = parentNode(); @@ -242,6 +261,11 @@ WbNode::~WbNode() { n = mParameters.size() - 1; for (int i = n; i >= 0; --i) delete mParameters[i]; + n = mInternalProtoParameters.size() - 1; + for (int i = n; i >= 0; --i) + delete mInternalProtoParameters[i]; + foreach (WbNodeProtoInfo *protoInfo, mProtoParents) + delete protoInfo; mProto->unref(); } @@ -1636,6 +1660,7 @@ WbNode *WbNode::createProtoInstanceFromParameters(WbProtoModel *proto, const QLi delete newNode; instance->mProto = proto; + instance->mProtoParents.prepend(new WbNodeProtoInfo(proto->name(), parameters)); if (id >= 0) instance->setUniqueId(id); @@ -1669,6 +1694,9 @@ WbNode *WbNode::createProtoInstanceFromParameters(WbProtoModel *proto, const QLi } gParent = tmpParent; + foreach (WbNodeProtoInfo *protoInfo, instance->mProtoParents) + protoInfo->redirectFields(param, aliasParam); + aliasNotFound = false; remove = true; } else if (aliasParam->name() == param->name()) { @@ -1733,13 +1761,25 @@ WbNode *WbNode::createProtoInstanceFromParameters(WbProtoModel *proto, const QLi instance->redirectAliasedFields(parameter, instance); } + // set the parent node of internal parameters + foreach (const WbField *f, instance->mInternalProtoParameters) { + QList internalFields = f->internalFields(); + while (!internalFields.isEmpty()) { + WbField *internalField = internalFields.takeFirst(); + internalField->setParentNode(instance); + internalFields << internalField->internalFields(); + } + } + // remove the fake parameters introduced in case of direct nested PROTOs QMutableVectorIterator fieldIt(instance->mParameters); while (fieldIt.hasNext()) { + // cppcheck-suppress constVariablePointer WbField *f = fieldIt.next(); if (!f->isHiddenParameter() && proto->findFieldModel(f->name()) == NULL) { fieldIt.remove(); - delete f; + // The field can still be accessed through the proto info, so don't delete it + instance->mInternalProtoParameters << f; } } delete gProtoParameterList.takeLast(); diff --git a/src/webots/vrml/WbNode.hpp b/src/webots/vrml/WbNode.hpp index 4d0fe855de4..c51f1a4fcaf 100644 --- a/src/webots/vrml/WbNode.hpp +++ b/src/webots/vrml/WbNode.hpp @@ -28,6 +28,7 @@ #include class WbNodeModel; +class WbNodeProtoInfo; class WbProtoModel; class WbField; class WbFieldModel; @@ -166,6 +167,7 @@ class WbNode : public QObject { bool isProtoInstance() const { return mProto != NULL; } WbProtoModel *proto() const { return mProto; } + const QList &protoParents() const { return mProtoParents; } bool isTemplate() const; void setRegenerationRequired(bool required); bool isRegenerationRequired() const { return mRegenerationRequired; } @@ -359,7 +361,9 @@ private slots: // for proto instances only WbProtoModel *mProto; + QList mProtoParents; QList mParameters; + QList mInternalProtoParameters; QByteArray mProtoInstanceTemplateContent; bool mRegenerationRequired; diff --git a/src/webots/vrml/WbNodeProtoInfo.cpp b/src/webots/vrml/WbNodeProtoInfo.cpp new file mode 100644 index 00000000000..b6f727fe3af --- /dev/null +++ b/src/webots/vrml/WbNodeProtoInfo.cpp @@ -0,0 +1,50 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// 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 +// +// https://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 "WbNodeProtoInfo.hpp" + +#include "WbField.hpp" + +static const WbFieldReference INVALID_FIELD_REFERENCE = {QString(), NULL}; + +WbNodeProtoInfo::WbNodeProtoInfo(const QString &modelName, const QList ¶meters) : mModelName(modelName) { + foreach (WbField *field, parameters) { + WbFieldReference ref; + ref.name = field->name(); + ref.actualField = field; + mParameters.append(ref); + } +} + +WbNodeProtoInfo::WbNodeProtoInfo(const WbNodeProtoInfo &other) : mModelName(other.mModelName), mParameters(other.mParameters) { +} + +const WbFieldReference &WbNodeProtoInfo::findFieldByIndex(int index) const { + if (index < 0 || index >= mParameters.size()) + return INVALID_FIELD_REFERENCE; + return mParameters.at(index); +} + +int WbNodeProtoInfo::findFieldIndex(const QString &name) const { + for (int i = 0; i < mParameters.size(); ++i) + if (mParameters.at(i).name == name) + return i; + return -1; +} + +void WbNodeProtoInfo::redirectFields(const WbField *oldField, WbField *newField) { + for (WbFieldReference &ref : mParameters) + if (ref.actualField == oldField) + ref.actualField = newField; +} diff --git a/src/webots/vrml/WbNodeProtoInfo.hpp b/src/webots/vrml/WbNodeProtoInfo.hpp new file mode 100644 index 00000000000..7bc26682f33 --- /dev/null +++ b/src/webots/vrml/WbNodeProtoInfo.hpp @@ -0,0 +1,52 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// 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 +// +// https://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 WB_NODE_PROTO_ANCESTRY_HPP +#define WB_NODE_PROTO_ANCESTRY_HPP + +// +// Description: class containing proto-specific information about a VRML node +// + +#include +#include + +class WbField; + +struct WbFieldReference { + QString name; + WbField *actualField; +}; + +class WbNodeProtoInfo { +public: + WbNodeProtoInfo(const QString &modelName, const QList ¶meters); + explicit WbNodeProtoInfo(const WbNodeProtoInfo &other); + + const QString &modelName() const { return mModelName; } + const QList ¶meters() const { return mParameters; } + + const WbFieldReference &findFieldByIndex(int index) const; + int findFieldIndex(const QString &name) const; + + void redirectFields(const WbField *oldField, WbField *newField); + +private: + WbNodeProtoInfo &operator=(const WbNodeProtoInfo &); // non copyable + + QString mModelName; + QList mParameters; +}; + +#endif diff --git a/tests/api/controllers/supervisor_field/supervisor_field.c b/tests/api/controllers/supervisor_field/supervisor_field.c index 3e5bbfe0265..5427f616a74 100644 --- a/tests/api/controllers/supervisor_field/supervisor_field.c +++ b/tests/api/controllers/supervisor_field/supervisor_field.c @@ -77,27 +77,25 @@ int main(int argc, char **argv) { d = wb_supervisor_field_get_sf_float(field); ts_assert_double_equal(d, 0.73, "Proto \"camera_fieldOfView\" SFFloat field should have value 0.73 not %f", d); - // invalid to retrive a non-PROTO field or a PROTO parameter - internal_field = wb_supervisor_node_get_proto_field(box, "scale"); - ts_assert_pointer_null(internal_field, "wb_supervisor_node_get_proto_field should only work for PROTO nodes."); - internal_field = wb_supervisor_node_get_proto_field(mfTest, "mfBool"); - ts_assert_pointer_null(internal_field, "wb_supervisor_node_get_proto_field should only work for internal PROTO fields."); + // invalid to retrieve a non-base-node field + internal_field = wb_supervisor_node_get_base_node_field(mfTest, "mfBool"); + ts_assert_pointer_null(internal_field, "wb_supervisor_node_get_base_node_field should not return PROTO parameters."); // internal SFNode - internal_sf_node_field = wb_supervisor_node_get_proto_field(proto, "physics"); + internal_sf_node_field = wb_supervisor_node_get_base_node_field(proto, "physics"); ts_assert_pointer_not_null(internal_sf_node_field, "wb_supervisor_node_get_proto_field should return an internal field."); internal_physics = wb_supervisor_field_get_sf_node(internal_sf_node_field); ts_assert_pointer_not_null(internal_physics, "wb_supervisor_field_get_sf_node should return an internal node."); internal_field = wb_supervisor_node_get_field(internal_physics, "density"); - ts_assert_pointer_not_null(internal_field, "wb_supervisor_node_get_proto_field should return an internal field."); + ts_assert_pointer_not_null(internal_field, "wb_supervisor_node_get_field should return an internal field."); d = wb_supervisor_field_get_sf_float(internal_field); ts_assert_double_equal(d, -1.0, "Returned value should be -1.0 and not %f", d); // internal SFFloat value internal_field = wb_supervisor_node_get_field(proto, "cpuConsumption"); ts_assert_pointer_null(internal_field, "wb_supervisor_node_get_field should not return internal fields."); - internal_field = wb_supervisor_node_get_proto_field(proto, "cpuConsumption"); - ts_assert_pointer_not_null(internal_field, "wb_supervisor_node_get_proto_field should return an internal field."); + internal_field = wb_supervisor_node_get_base_node_field(proto, "cpuConsumption"); + ts_assert_pointer_not_null(internal_field, "wb_supervisor_node_get_base_node_field should return an internal field."); d = wb_supervisor_field_get_sf_float(internal_field); ts_assert_double_equal(d, 1.11, "Returned value should be 1.11 and not %f", d); wb_supervisor_field_set_sf_float(internal_field, 1.5); @@ -430,7 +428,7 @@ int main(int argc, char **argv) { int internal_field_type = wb_supervisor_field_get_type(internal_field); printf("internal_field_type %d\n", internal_field_type); ts_assert_int_equal(internal_field_type, 0, "Internal field reference is invalid after PROTO regeneration"); - internal_field = wb_supervisor_node_get_proto_field(proto, "cpuConsumption"); + internal_field = wb_supervisor_node_get_base_node_field(proto, "cpuConsumption"); printf("internal_field %p\n", internal_field); ts_assert_pointer_not_null(internal_field, "Node reference should be invalid after PROTO regeneration"); d = wb_supervisor_field_get_sf_float(internal_field); @@ -469,25 +467,22 @@ int main(int argc, char **argv) { fieldInvalid = wb_supervisor_node_get_field_by_index(mfTest, fields_count); ts_assert_pointer_null(fieldInvalid, "It should not be possible to retrieve a field using an out of range index"); - const int proto_fields_count = wb_supervisor_node_get_proto_number_of_fields(mfTest); - ts_assert_int_equal(proto_fields_count, 17, "Number of PROTO internal fields of MF_FIELDS node is wrong"); - field0 = wb_supervisor_node_get_proto_field_by_index(mfTest, 0); + const int proto_fields_count = wb_supervisor_node_get_number_of_base_node_fields(mfTest); + ts_assert_int_equal(proto_fields_count, 17, "Number of base node fields of MF_FIELDS node is wrong"); + field0 = wb_supervisor_node_get_base_node_field_by_index(mfTest, 0); ts_assert_string_equal(wb_supervisor_field_get_name(field0), "translation", - "Name of first PROTO internal field of MF_FIELDS node is wrong: \"%s\" should be \"translation\"", - field0); - field2 = wb_supervisor_node_get_proto_field_by_index(mfTest, 2); + "Name of first base node field of MF_FIELDS node is wrong: \"%s\" should be \"translation\"", field0); + field2 = wb_supervisor_node_get_base_node_field_by_index(mfTest, 2); ts_assert_string_equal(wb_supervisor_field_get_name(field2), "children", - "Name of third PROTO internal field of MF_FIELDS node is wrong: \"%s\" should be \"children\"", - field2); - field8 = wb_supervisor_node_get_proto_field_by_index(mfTest, fields_count - 1); + "Name of third base node field of MF_FIELDS node is wrong: \"%s\" should be \"children\"", field2); + field8 = wb_supervisor_node_get_base_node_field_by_index(mfTest, fields_count - 1); ts_assert_string_equal(wb_supervisor_field_get_name(field8), "boundingObject", - "Name of ninth PROTO internal field of MF_FIELDS node is wrong: \"%s\" should be \"boundingObject\"", + "Name of ninth base node field of MF_FIELDS node is wrong: \"%s\" should be \"boundingObject\"", field8); - fieldInvalid = wb_supervisor_node_get_proto_field_by_index(mfTest, -5); - ts_assert_pointer_null(fieldInvalid, "It should not be possible to retrieve a PROTO internal field using a negative index"); - fieldInvalid = wb_supervisor_node_get_proto_field_by_index(mfTest, proto_fields_count); - ts_assert_pointer_null(fieldInvalid, - "It should not be possible to retrieve a PROTO internal field using an out of range index"); + fieldInvalid = wb_supervisor_node_get_base_node_field_by_index(mfTest, -5); + ts_assert_pointer_null(fieldInvalid, "It should not be possible to retrieve a base node field using a negative index"); + fieldInvalid = wb_supervisor_node_get_base_node_field_by_index(mfTest, proto_fields_count); + ts_assert_pointer_null(fieldInvalid, "It should not be possible to retrieve a base node field using an out of range index"); wb_robot_step(TIME_STEP); diff --git a/tests/api/controllers/supervisor_import_remove_mf/supervisor_import_remove_mf.c b/tests/api/controllers/supervisor_import_remove_mf/supervisor_import_remove_mf.c index 708121e0671..150ac458528 100644 --- a/tests/api/controllers/supervisor_import_remove_mf/supervisor_import_remove_mf.c +++ b/tests/api/controllers/supervisor_import_remove_mf/supervisor_import_remove_mf.c @@ -38,49 +38,55 @@ int main(int argc, char **argv) { // Import wb_robot_step(TIME_STEP); - wb_supervisor_field_insert_mf_bool(bool_field, 0, true); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFBool incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_int32(int_field, -1, 17); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFInt32 incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_float(float_field, 0, 0.54); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFFloat incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_vec2f(vec2_field, -1, vec2_value); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFVec2f incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_vec3f(vec3_field, 0, vec3_value); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFVec3f incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_rotation(rotation_field, -1, rotation_value); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFRotation incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_color(color_field, 0, color_value); reference_distance -= 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after insertion of MFColor incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_insert_mf_string(string_field, -1, "Hello World!"); const char *expected_string = "Second string"; wb_supervisor_field_insert_mf_string(string_field, 1, expected_string); @@ -93,48 +99,56 @@ int main(int argc, char **argv) { // Remove + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(string_field, 0); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFString incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(color_field, -1); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFColor incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(rotation_field, -1); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFRotation incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(vec3_field, 0); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFVec3f incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(vec2_field, 0); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFVec2f incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(float_field, -1); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFFloat incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(int_field, 0); reference_distance += 0.05; wb_robot_step(TIME_STEP); ts_assert_double_in_delta(wb_distance_sensor_get_value(distance_sensor), reference_distance, 0.0001, "Distance after removing MFInt32 incorrect."); + wb_robot_step(TIME_STEP); wb_supervisor_field_remove_mf(bool_field, -1); reference_distance += 0.05; wb_robot_step(TIME_STEP); diff --git a/tests/api/controllers/supervisor_notify_import_remove_mf/supervisor_notify_import_remove_mf.c b/tests/api/controllers/supervisor_notify_import_remove_mf/supervisor_notify_import_remove_mf.c index f2431aa201b..c2ee204287b 100644 --- a/tests/api/controllers/supervisor_notify_import_remove_mf/supervisor_notify_import_remove_mf.c +++ b/tests/api/controllers/supervisor_notify_import_remove_mf/supervisor_notify_import_remove_mf.c @@ -12,38 +12,71 @@ int main(int argc, char **argv) { int i = 0; WbNodeRef node = wb_supervisor_node_get_from_def("NODE"); - const int FIELD_COUNT = 8; - WbFieldRef mfField[FIELD_COUNT]; - mfField[0] = wb_supervisor_node_get_field(node, "bool"); - mfField[1] = wb_supervisor_node_get_field(node, "int"); - mfField[2] = wb_supervisor_node_get_field(node, "float"); - mfField[3] = wb_supervisor_node_get_field(node, "vec2"); - mfField[4] = wb_supervisor_node_get_field(node, "vec3"); - mfField[5] = wb_supervisor_node_get_field(node, "rot"); - mfField[6] = wb_supervisor_node_get_field(node, "color"); - mfField[7] = wb_supervisor_node_get_field(node, "string"); - int mfFieldCount[FIELD_COUNT]; - for (i = 0; i < FIELD_COUNT; ++i) - mfFieldCount[i] = wb_supervisor_field_get_count(mfField[i]); + WbProtoRef proto = wb_supervisor_node_get_proto(node); + const char *field_names[] = {"bool", "int", "float", "vec2", "vec3", "rot", "color", "string"}; + const int FIELD_COUNT = sizeof(field_names) / sizeof(field_names[0]); + WbFieldRef mf_field[FIELD_COUNT]; + WbFieldRef mf_proto_fields[FIELD_COUNT]; + for (i = 0; i < FIELD_COUNT; ++i) { + mf_field[i] = wb_supervisor_node_get_field(node, field_names[i]); + mf_proto_fields[i] = wb_supervisor_proto_get_field(proto, field_names[i]); + } + + int mf_field_count[FIELD_COUNT]; + for (i = 0; i < FIELD_COUNT; ++i) { + const int count = wb_supervisor_field_get_count(mf_field[i]); + mf_field_count[i] = count; + const int proto_count = wb_supervisor_field_get_count(mf_proto_fields[i]); + ts_assert_int_equal(proto_count, count, "Size of proto field %d not correctly initialized: found %d, expected %d", i, + proto_count, count); + } wb_robot_step(3 * TIME_STEP); for (i = 0; i < FIELD_COUNT; ++i) { wb_robot_step(TIME_STEP); - const int count = wb_supervisor_field_get_count(mfField[i]); + const int count = wb_supervisor_field_get_count(mf_field[i]); int increment = i < 7 ? 1 : 2; - ts_assert_int_equal(count, mfFieldCount[i] + increment, + ts_assert_int_equal(count, mf_field_count[i] + increment, "Size of field %d not correctly updated after item inserted: found %d, expected %d", i, count, - mfFieldCount[i] + increment); + mf_field_count[i] + increment); + + // The proto node should have been regenerated, so all the references should be invalidated + ts_assert_string_equal(wb_supervisor_proto_get_type_name(proto), "", + "Proto node should have been regenerated after field insertion."); + ts_assert_int_equal(wb_supervisor_field_get_count(mf_proto_fields[i]), -1, + "Proto field %d should have been invalidated after field insertion.", i); + proto = wb_supervisor_node_get_proto(node); + mf_proto_fields[i] = wb_supervisor_proto_get_field(proto, field_names[i]); + + const int proto_count = wb_supervisor_field_get_count(mf_proto_fields[i]); + ts_assert_int_equal(proto_count, mf_field_count[i] + increment, + "Size of proto field %d not correctly updated after item inserted: found %d, expected %d", i, + proto_count, mf_field_count[i] + increment); + wb_robot_step(TIME_STEP); } for (i = FIELD_COUNT - 1; i >= 0; --i) { wb_robot_step(TIME_STEP); - const int count = wb_supervisor_field_get_count(mfField[i]); + const int count = wb_supervisor_field_get_count(mf_field[i]); int increment = i < 7 ? 0 : 1; - ts_assert_int_equal(count, mfFieldCount[i] + increment, + ts_assert_int_equal(count, mf_field_count[i] + increment, "Size of field %d not correctly updated after item removed: found %d, expected %d", i, count, - mfFieldCount[i] + increment); + mf_field_count[i] + increment); + + // The proto node should have been regenerated, so all the references should be invalidated + ts_assert_string_equal(wb_supervisor_proto_get_type_name(proto), "", + "Proto node should have been regenerated after field removal."); + ts_assert_int_equal(wb_supervisor_field_get_count(mf_proto_fields[i]), -1, + "Proto field %d should have been invalidated after field removal.", i); + proto = wb_supervisor_node_get_proto(node); + mf_proto_fields[i] = wb_supervisor_proto_get_field(proto, field_names[i]); + + const int proto_count = wb_supervisor_field_get_count(mf_proto_fields[i]); + ts_assert_int_equal(proto_count, mf_field_count[i] + increment, + "Size of proto field %d not correctly updated after item removed: found %d, expected %d", i, + proto_count, mf_field_count[i] + increment); + wb_robot_step(TIME_STEP); } ts_send_success(); diff --git a/tests/api/controllers/supervisor_proto/.gitignore b/tests/api/controllers/supervisor_proto/.gitignore new file mode 100644 index 00000000000..af96de1dba8 --- /dev/null +++ b/tests/api/controllers/supervisor_proto/.gitignore @@ -0,0 +1 @@ +/supervisor_proto diff --git a/tests/api/controllers/supervisor_proto/Makefile b/tests/api/controllers/supervisor_proto/Makefile new file mode 100644 index 00000000000..137d83b4e39 --- /dev/null +++ b/tests/api/controllers/supervisor_proto/Makefile @@ -0,0 +1,25 @@ +# Copyright 1996-2023 Cyberbotics Ltd. +# +# 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 +# +# https://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. + +# Webots Makefile system +# +# You may add some variable definitions hereafter to customize the build process +# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include + + +# Do not modify the following: this includes Webots global Makefile.include +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/tests/api/controllers/supervisor_proto/supervisor_proto.c b/tests/api/controllers/supervisor_proto/supervisor_proto.c new file mode 100644 index 00000000000..3dd8da751b3 --- /dev/null +++ b/tests/api/controllers/supervisor_proto/supervisor_proto.c @@ -0,0 +1,150 @@ +/* + * Description: Test Supervisor API + * This file tests the basic functionalities of the proto api + * such as reading the proto hierarchy and verifying the fields + * of the proto nodes. + * Because this test shares the world with supervisor_proto_fields, + * which tests the ability to modify the fields of a proto node, + * it should not assume or modify the field values. + */ + +#include +#include + +#include "../../../lib/ts_assertion.h" +#include "../../../lib/ts_utils.h" + +#define TIME_STEP 32 + +struct FieldDefinition { + const char *name; + WbFieldType type; +}; + +static void assert_hierarchy_correct(WbProtoRef proto, const char **expected_hierarchy, const char *node_name) { + for (int i = 0; expected_hierarchy[i]; ++i) { + const char *actual_name = wb_supervisor_proto_get_type_name(proto); + ts_assert_pointer_not_null(actual_name, "(%s) Hierarchy mismatch! Expected \"%s\", but got NULL", node_name, + expected_hierarchy[i]); + ts_assert_string_equal(actual_name, expected_hierarchy[i], "(%s) Hierarchy mismatch! Expected \"%s\", but got \"%s\"", + node_name, expected_hierarchy[i], actual_name); + + if (expected_hierarchy[i + 1]) + ts_assert_boolean_equal(wb_supervisor_proto_is_derived(proto), + "(%s) wb_supervisor_proto_is_derived() should return true for \"%s\"", node_name, actual_name); + else + ts_assert_boolean_not_equal(wb_supervisor_proto_is_derived(proto), + "(%s) wb_supervisor_proto_is_derived() should return false for \"%s\"", node_name, + actual_name); + + proto = wb_supervisor_proto_get_parent(proto); + } + ts_assert_pointer_null(proto, "(%s) Hierarchy mismatch! Expected NULL, but got \"%s\"", node_name, + wb_supervisor_proto_get_type_name(proto)); +} + +static void assert_fields_correct(WbProtoRef proto, const struct FieldDefinition **expected_fields, const char *node_name) { + for (int i = 0; expected_fields[i]; ++i) { + ts_assert_pointer_not_null(proto, + "(%s) Expected more fields, but proto is null. The test should've failed when checking the " + "hierarchy. This likely means that the expected values are incorrectly configured.", + node_name); + + int number_of_fields = 0; + while (expected_fields[i][number_of_fields].type != WB_NO_FIELD) + ++number_of_fields; + + const char *proto_name = wb_supervisor_proto_get_type_name(proto); + + const int actual_number_of_fields = wb_supervisor_proto_get_number_of_fields(proto); + ts_assert_int_equal(actual_number_of_fields, number_of_fields, + "(%s) Wrong number of fields in proto \"%s\". Expected %d, but got %d", node_name, proto_name, + number_of_fields, actual_number_of_fields); + + for (int j = 0; j < number_of_fields; ++j) { + const char *expected_name = expected_fields[i][j].name; + const WbFieldType expected_type = expected_fields[i][j].type; + + WbFieldRef field = wb_supervisor_proto_get_field_by_index(proto, j); + ts_assert_pointer_not_null( + field, "(%s) Field \"%d\" not found in proto \"%s\" despite the reported number of fields being correct", node_name, j, + proto_name); + + const char *actual_name = wb_supervisor_field_get_name(field); + ts_assert_string_equal(actual_name, expected_name, "(%s) Field mismatch in proto \"%s\"! Expected \"%s\", but got \"%s\"", + node_name, proto_name, expected_name, actual_name); + + const WbFieldType actual_type = wb_supervisor_field_get_type(field); + ts_assert_int_equal(actual_type, expected_type, + "(%s) Field \"%s\" in proto \"%s\" has wrong type! Expected %d, but got %d", node_name, expected_name, + proto_name, expected_type, actual_type); + } + + proto = wb_supervisor_proto_get_parent(proto); + } +} + +int main(int argc, char **argv) { + ts_setup(argv[0]); + + // Check null values + ts_assert_pointer_null(wb_supervisor_node_get_proto(NULL), "wb_supervisor_node_get_proto(NULL) should return NULL"); + ts_assert_string_equal(wb_supervisor_proto_get_type_name(NULL), "", + "wb_supervisor_proto_get_type_name(NULL) should return NULL"); + ts_assert_boolean_not_equal(wb_supervisor_proto_is_derived(NULL), "wb_supervisor_proto_is_derived(NULL) should return false"); + ts_assert_pointer_null(wb_supervisor_proto_get_parent(NULL), "wb_supervisor_proto_get_parent(NULL) should return NULL"); + ts_assert_int_equal(wb_supervisor_proto_get_number_of_fields(NULL), -1, + "wb_supervisor_proto_get_number_of_fields(NULL) should return -1"); + ts_assert_pointer_null(wb_supervisor_proto_get_field_by_index(NULL, 0), + "wb_supervisor_proto_get_field_by_index(NULL, 0) should return NULL"); + ts_assert_pointer_null(wb_supervisor_proto_get_field(NULL, "name"), + "wb_supervisor_proto_get_field(NULL, \"name\") should return NULL"); + + // This array keeps track of the names of the proto types we expect at each level of the hierarchy, + // starting with the proto type of the main hierarchy node, and ending with NULL. + const char *expected_hierarchy[] = {"SolidProtoHierarchy", "SolidProtoHierarchyInternal", "SolidProtoHierarchyBase", NULL}; + // This array keeps track of the fields we expect at each level of the hierarchy. + // Each element in this array is an array that represents a single proto type + // Each of these arrays contains the fields we expect to find in that type, in the order we expect them, + // terminated with a field of type WB_NO_FIELD. + const struct FieldDefinition *expected_fields[] = {(const struct FieldDefinition[]){// SolidProtoHierarchy + {"translationStep", WB_SF_FLOAT}, + {"internalTranslationStep", WB_SF_FLOAT}, + {"internalField1", WB_SF_FLOAT}, + {"internalField2", WB_SF_FLOAT}, + {"ucField4", WB_SF_FLOAT}, + {NULL, WB_NO_FIELD}}, + (const struct FieldDefinition[]){// SolidProtoInternal + {"translationStep", WB_SF_FLOAT}, + {"rotationStep", WB_SF_FLOAT}, + {"children", WB_MF_NODE}, + {"ucField1", WB_SF_FLOAT}, + {"ucField2", WB_SF_FLOAT}, + {"ucField3", WB_SF_FLOAT}, + {NULL, WB_NO_FIELD}}, + (const struct FieldDefinition[]){// SolidProtoBase + {"translationStep", WB_SF_FLOAT}, + {"rotationStep", WB_SF_FLOAT}, + {"children", WB_MF_NODE}, + {"ucField1", WB_SF_FLOAT}, + {NULL, WB_NO_FIELD}}, + NULL}; + + WbNodeRef hierarchy = wb_supervisor_node_get_from_def("HIERARCHY"); + ts_assert_pointer_not_null(hierarchy, "Hierarchy node not found"); + WbNodeRef internal_node = wb_supervisor_node_get_from_proto_def(hierarchy, "INTERNAL_NODE"); + ts_assert_pointer_not_null(internal_node, "Internal node not found"); + + WbProtoRef hierarchy_proto = wb_supervisor_node_get_proto(hierarchy); + WbProtoRef internal_proto = wb_supervisor_node_get_proto(internal_node); + + assert_hierarchy_correct(hierarchy_proto, expected_hierarchy, "HIERARCHY"); + assert_fields_correct(hierarchy_proto, expected_fields, "HIERARCHY"); + // Because the main hierarchy node and the internal node are both of type SolidProtoInternal, + // we can just skip the first element of both arrays when validating the internal node + assert_hierarchy_correct(internal_proto, expected_hierarchy + 1, "INTERNAL_NODE"); + assert_fields_correct(internal_proto, expected_fields + 1, "INTERNAL_NODE"); + + ts_send_success(); + return EXIT_SUCCESS; +} diff --git a/tests/api/controllers/supervisor_proto_fields/.gitignore b/tests/api/controllers/supervisor_proto_fields/.gitignore new file mode 100644 index 00000000000..ece47a86b9e --- /dev/null +++ b/tests/api/controllers/supervisor_proto_fields/.gitignore @@ -0,0 +1 @@ +/supervisor_proto_fields diff --git a/tests/api/controllers/supervisor_proto_fields/Makefile b/tests/api/controllers/supervisor_proto_fields/Makefile new file mode 100644 index 00000000000..137d83b4e39 --- /dev/null +++ b/tests/api/controllers/supervisor_proto_fields/Makefile @@ -0,0 +1,25 @@ +# Copyright 1996-2023 Cyberbotics Ltd. +# +# 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 +# +# https://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. + +# Webots Makefile system +# +# You may add some variable definitions hereafter to customize the build process +# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include + + +# Do not modify the following: this includes Webots global Makefile.include +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/tests/api/controllers/supervisor_proto_fields/supervisor_proto_fields.c b/tests/api/controllers/supervisor_proto_fields/supervisor_proto_fields.c new file mode 100644 index 00000000000..6377452b73c --- /dev/null +++ b/tests/api/controllers/supervisor_proto_fields/supervisor_proto_fields.c @@ -0,0 +1,287 @@ +/* + * Description: Test Supervisor API + * This file tests the ability to read and set the values of a proto node's fields. + * For simplicity, all fields are SFFloats. This test only needs to check that the + * appropriate access restrictions are in place for proto fields. The actual get/set + * functionality for other types of fields is tested in supervisor_field. + */ + +#include +#include + +#include "../../../lib/ts_assertion.h" +#include "../../../lib/ts_utils.h" + +#define TIME_STEP 32 + +#define NUMBER_OF_MAIN_FIELDS 7 +#define NUMBER_OF_INTERNAL_FIELDS 6 +#define NUMBER_OF_HIERARCHY_LEVELS 4 + +// Field/Proto references may become invalid if any nodes are regenerated. This function provides an easy way to re-retrieve +// them. +// At the time of writing, the protos being used are not templated, so they shouldn't be regenerated. However, this function +// is still useful for future-proofing and for segmenting the retrieval code from the main test. +static void retrieve_fields(const char *main_field_names[NUMBER_OF_MAIN_FIELDS][NUMBER_OF_HIERARCHY_LEVELS], + const char *internal_field_names[NUMBER_OF_INTERNAL_FIELDS][NUMBER_OF_HIERARCHY_LEVELS], + WbNodeRef hierarchy, WbFieldRef actual_main_fields[NUMBER_OF_MAIN_FIELDS], + WbFieldRef actual_internal_fields[NUMBER_OF_INTERNAL_FIELDS], + WbFieldRef main_fields[NUMBER_OF_MAIN_FIELDS][NUMBER_OF_HIERARCHY_LEVELS], + WbFieldRef internal_fields[NUMBER_OF_INTERNAL_FIELDS][NUMBER_OF_HIERARCHY_LEVELS]) { + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) { + const char *name = main_field_names[i][0]; + + if (!name) { + // No top-level field exists + actual_main_fields[i] = NULL; + continue; + } + + WbFieldRef field = wb_supervisor_node_get_field(hierarchy, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found", name); + actual_main_fields[i] = wb_supervisor_field_get_actual_field(field); + } + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) { + const char *name = internal_field_names[i][0]; + + if (!name) { + // No top-level field exists + actual_internal_fields[i] = NULL; + continue; + } + + // All of the actual fields are part of the hierarchy node in the scene tree + WbFieldRef field = wb_supervisor_node_get_field(hierarchy, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found", name); + actual_internal_fields[i] = wb_supervisor_field_get_actual_field(field); + } + + WbProtoRef hierarchy_proto = wb_supervisor_node_get_proto(hierarchy); + ts_assert_pointer_not_null(hierarchy_proto, "Hierarchy proto not found"); + + // The first level of the internal hierarchy is the same as the main hierarchy + // We handle it here, before we start messing with hierarchy_proto + const char *proto_type = wb_supervisor_proto_get_type_name(hierarchy_proto); + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) { + const char *field_name = internal_field_names[i][0]; + if (field_name) { + WbFieldRef field = wb_supervisor_proto_get_field(hierarchy_proto, field_name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found in proto of type \"%s\"", field_name, proto_type); + internal_fields[i][0] = field; + } else + // The field does not exist at this level of the hierarchy + internal_fields[i][0] = NULL; + } + + // Fetch all the internal fields in the main hierarchy + for (int i = 0; i < NUMBER_OF_HIERARCHY_LEVELS - 1; ++i) { + ts_assert_pointer_not_null( + hierarchy_proto, "Hierarchy level %d does not exist. Try running the supervisor_proto test for more information.", i); + proto_type = wb_supervisor_proto_get_type_name(hierarchy_proto); + for (int j = 0; j < NUMBER_OF_MAIN_FIELDS; ++j) { + const char *name = main_field_names[j][i]; + if (name) { + WbFieldRef field = wb_supervisor_proto_get_field(hierarchy_proto, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found in proto of type \"%s\"", name, proto_type); + main_fields[j][i] = field; + } else + // The field does not exist at this level of the hierarchy + main_fields[j][i] = NULL; + } + hierarchy_proto = wb_supervisor_proto_get_parent(hierarchy_proto); + } + + // The last expected hierarchy level refers to the base node + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) { + const char *name = main_field_names[i][NUMBER_OF_HIERARCHY_LEVELS - 1]; + if (name) { + WbFieldRef field = wb_supervisor_node_get_base_node_field(hierarchy, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found in base hierarchy node", name); + main_fields[i][NUMBER_OF_HIERARCHY_LEVELS - 1] = field; + } else + // The field does not exist at this level of the hierarchy + main_fields[i][NUMBER_OF_HIERARCHY_LEVELS - 1] = NULL; + } + + WbNodeRef internal_node = wb_supervisor_node_get_from_proto_def(hierarchy, "INTERNAL_NODE"); + ts_assert_pointer_not_null(internal_node, "Internal node not found"); + WbProtoRef internal_proto = wb_supervisor_node_get_proto(internal_node); + ts_assert_pointer_not_null(internal_proto, "Internal node proto not found"); + + // Fetch all the internal fields in the internal node hierarchy + for (int i = 1; i < NUMBER_OF_HIERARCHY_LEVELS - 1; ++i) { + ts_assert_pointer_not_null( + internal_proto, "Hierarchy level %d does not exist. Try running the supervisor_proto test for more information.", i); + proto_type = wb_supervisor_proto_get_type_name(internal_proto); + for (int j = 0; j < NUMBER_OF_INTERNAL_FIELDS; ++j) { + const char *name = internal_field_names[j][i]; + if (name) { + WbFieldRef field = wb_supervisor_proto_get_field(internal_proto, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found in proto of type \"%s\"", name, proto_type); + internal_fields[j][i] = field; + } else + // The field does not exist at this level of the hierarchy + internal_fields[j][i] = NULL; + } + internal_proto = wb_supervisor_proto_get_parent(internal_proto); + } + + // The last expected hierarchy level refers to the base node + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) { + const char *name = internal_field_names[i][NUMBER_OF_HIERARCHY_LEVELS - 1]; + if (name) { + WbFieldRef field = wb_supervisor_node_get_base_node_field(internal_node, name); + ts_assert_pointer_not_null(field, "Field \"%s\" not found in base internal node", name); + internal_fields[i][NUMBER_OF_HIERARCHY_LEVELS - 1] = field; + } else + // The field does not exist at this level of the hierarchy + internal_fields[i][NUMBER_OF_HIERARCHY_LEVELS - 1] = NULL; + } +} + +// Checks that a field has the expected type, actual field, and value +static void check_field(WbFieldRef field, WbFieldRef actual_field, double expected_value, const char *name, + const char *check_name) { + ts_assert_pointer_not_null(field, "(%s) Field \"%s\" not found", check_name, name); + const int field_type = wb_supervisor_field_get_type(field); + ts_assert_int_equal(field_type, WB_SF_FLOAT, "(%s) Field \"%s\" is not an SFFloat. Got type: %d", check_name, name, + field_type); + ts_assert_boolean_equal(wb_supervisor_field_get_actual_field(field) == actual_field, + "(%s) Field \"%s\" actual field mismatch", check_name, name); + const double actual_value = wb_supervisor_field_get_sf_float(field); + ts_assert_double_equal(actual_value, expected_value, "(%s) Field \"%s\" value mismatch! Expected %f, but got %f", check_name, + name, expected_value, actual_value); +} + +int main(int argc, char **argv) { + ts_setup(argv[0]); + + // This array keeps track of the names of the fields we want to test at each level of the main node hierarchy. + // Each element in this array is an array that represents a single field, which may exist at many levels of the hierarchy. + // Each of these arrays contains the name for that field at each level of the hierarchy, from the main hierarchy node + // up to AND INCLUDING the base node type. All levels in the hierarchy are assumed to be present in the array. + // If a field does not exist at a specific level in the hierarchy, it should be listed as NULL + const char *main_field_names[NUMBER_OF_MAIN_FIELDS][NUMBER_OF_HIERARCHY_LEVELS] = { + {"translationStep", "translationStep", "translationStep", "translationStep"}, + {"internalField1", "ucField2", NULL, NULL}, + {"ucField4", NULL, NULL, NULL}, + {NULL, "rotationStep", "rotationStep", "rotationStep"}, + {NULL, "ucField1", "ucField1", NULL}, + {NULL, "ucField3", NULL, NULL}, + {NULL, NULL, NULL, "radarCrossSection"}, + }; + // This array is the same as the above array, except, with the exception of the first entry in each sub-array, + // all fields are assumed to be present in the in internal node rather than the main hierarchy node. + const char *internal_field_names[NUMBER_OF_INTERNAL_FIELDS][NUMBER_OF_HIERARCHY_LEVELS] = { + {"internalTranslationStep", "translationStep", "translationStep", "translationStep"}, + {"internalField2", "ucField2", NULL, NULL}, + {NULL, "rotationStep", "rotationStep", "rotationStep"}, + {NULL, "ucField1", "ucField1", NULL}, + {NULL, "ucField3", NULL, NULL}, + {NULL, NULL, NULL, "radarCrossSection"}, + }; + + WbNodeRef hierarchy = wb_supervisor_node_get_from_def("HIERARCHY"); + ts_assert_pointer_not_null(hierarchy, "Hierarchy node not found"); + + WbFieldRef actual_main_fields[NUMBER_OF_MAIN_FIELDS]; + WbFieldRef actual_internal_fields[NUMBER_OF_INTERNAL_FIELDS]; + WbFieldRef main_fields[NUMBER_OF_MAIN_FIELDS][NUMBER_OF_HIERARCHY_LEVELS]; + WbFieldRef internal_fields[NUMBER_OF_INTERNAL_FIELDS][NUMBER_OF_HIERARCHY_LEVELS]; + + retrieve_fields(main_field_names, internal_field_names, hierarchy, actual_main_fields, actual_internal_fields, main_fields, + internal_fields); + + // Check that all the wb_supervisor_field_get_actual_field returns its input if the field is already in the scene tree + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) + if (actual_main_fields[i]) + check_field(actual_main_fields[i], actual_main_fields[i], 0.01, main_field_names[i][0], "Initial value [main field]"); + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) + if (actual_internal_fields[i]) + check_field(actual_internal_fields[i], actual_internal_fields[i], 0.01, internal_field_names[i][0], + "Initial value [internal field]"); + + // Check that wb_supervisor_field_get_actual_field returns the correct field (or NULL) in all other cases, and that internal + // fields are read-only + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (main_fields[i][j]) { + check_field(main_fields[i][j], actual_main_fields[i], 0.01, main_field_names[i][j], "Initial value [main field]"); + wb_supervisor_field_set_sf_float(main_fields[i][j], 1.01); // Should have no effect + } + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (internal_fields[i][j]) { + check_field(internal_fields[i][j], actual_internal_fields[i], 0.01, internal_field_names[i][j], + "Initial value [internal field]"); + wb_supervisor_field_set_sf_float(internal_fields[i][j], 1.01); // Should have no effect + } + + wb_robot_step(TIME_STEP); + + // Nothing should have changed, but just in case, re-retrieve the fields + retrieve_fields(main_field_names, internal_field_names, hierarchy, actual_main_fields, actual_internal_fields, main_fields, + internal_fields); + + // Check that no fields were modified + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) { + if (actual_main_fields[i]) + check_field(actual_main_fields[i], actual_main_fields[i], 0.01, main_field_names[i][0], + "Update read-only field [main field]"); + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (main_fields[i][j]) + check_field(main_fields[i][j], actual_main_fields[i], 0.01, main_field_names[i][j], + "Update read-only field [main field]"); + } + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) { + if (actual_internal_fields[i]) + check_field(actual_internal_fields[i], actual_internal_fields[i], 0.01, internal_field_names[i][0], + "Update read-only field [internal field]"); + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (internal_fields[i][j]) + check_field(internal_fields[i][j], actual_internal_fields[i], 0.01, internal_field_names[i][j], + "Update read-only field [internal field]"); + } + + // Update the fields + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) + if (actual_main_fields[i]) + wb_supervisor_field_set_sf_float(actual_main_fields[i], i); + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) + if (actual_internal_fields[i]) + wb_supervisor_field_set_sf_float(actual_internal_fields[i], i); + + wb_robot_step(TIME_STEP); + + // The nodes may have been regenerated. Update all the relevant references + retrieve_fields(main_field_names, internal_field_names, hierarchy, actual_main_fields, actual_internal_fields, main_fields, + internal_fields); + + // Check that the fields were updated correctly + for (int i = 0; i < NUMBER_OF_MAIN_FIELDS; ++i) { + if (actual_main_fields[i]) + check_field(actual_main_fields[i], actual_main_fields[i], i, main_field_names[i][0], "Update field [main field]"); + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (main_fields[i][j]) + check_field(main_fields[i][j], actual_main_fields[i], actual_main_fields[i] ? i : 0.01, main_field_names[i][j], + "Update field [main field]"); + } + + for (int i = 0; i < NUMBER_OF_INTERNAL_FIELDS; ++i) { + if (actual_internal_fields[i]) + check_field(actual_internal_fields[i], actual_internal_fields[i], i, internal_field_names[i][0], + "Update field [internal field]"); + for (int j = 0; j < NUMBER_OF_HIERARCHY_LEVELS; ++j) + if (internal_fields[i][j]) + check_field(internal_fields[i][j], actual_internal_fields[i], actual_internal_fields[i] ? i : 0.01, + internal_field_names[i][j], "Update field [internal field]"); + } + + ts_send_success(); + return EXIT_SUCCESS; +} diff --git a/tests/api/protos/SolidProtoHierarchy.proto b/tests/api/protos/SolidProtoHierarchy.proto new file mode 100644 index 00000000000..3e47d0d5162 --- /dev/null +++ b/tests/api/protos/SolidProtoHierarchy.proto @@ -0,0 +1,26 @@ +#VRML_SIM R2024a utf8 + +EXTERNPROTO "webots://tests/api/protos/SolidProtoHierarchyInternal.proto" + +PROTO SolidProtoHierarchy [ + field SFFloat translationStep 0.01 + field SFFloat internalTranslationStep 0.01 + field SFFloat internalField1 0.01 + field SFFloat internalField2 0.01 + unconnectedField SFFloat ucField4 0.01 +] +{ + SolidProtoHierarchyInternal { + translationStep IS translationStep + ucField2 IS internalField1 + # Note: For the purposes of the test, SolidProtoHierarchyInternal.rotationStep, + # SolidProtoHierarchyInternal.ucField1, and SolidProtoHierarchyInternal.ucField2 are explicitly left unconnected + children [ + DEF INTERNAL_NODE SolidProtoHierarchyInternal { + translationStep IS internalTranslationStep + ucField2 IS internalField2 + # See above comment about rotationStep, ucField1, and ucField2 + } + ] + } +} diff --git a/tests/api/protos/SolidProtoHierarchyBase.proto b/tests/api/protos/SolidProtoHierarchyBase.proto new file mode 100644 index 00000000000..7fe8cf15a04 --- /dev/null +++ b/tests/api/protos/SolidProtoHierarchyBase.proto @@ -0,0 +1,17 @@ +#VRML_SIM R2024a utf8 + +PROTO SolidProtoHierarchyBase [ + field SFFloat translationStep 0.01 + field SFFloat rotationStep 0.01 + field MFNode children [] + unconnectedField SFFloat ucField1 0.01 + # Note: For the purposes of the test, radarCrossSection is explicitly not exposed +] +{ + Solid { + translationStep IS translationStep + rotationStep IS rotationStep + children IS children + radarCrossSection 0.01 + } +} diff --git a/tests/api/protos/SolidProtoHierarchyInternal.proto b/tests/api/protos/SolidProtoHierarchyInternal.proto new file mode 100644 index 00000000000..65a1ac04c4f --- /dev/null +++ b/tests/api/protos/SolidProtoHierarchyInternal.proto @@ -0,0 +1,20 @@ +#VRML_SIM R2024a utf8 + +EXTERNPROTO "webots://tests/api/protos/SolidProtoHierarchyBase.proto" + +PROTO SolidProtoHierarchyInternal [ + field SFFloat translationStep 0.01 + field SFFloat rotationStep 0.01 + field MFNode children [] + field SFFloat ucField1 0.01 + unconnectedField SFFloat ucField2 0.01 + unconnectedField SFFloat ucField3 0.01 +] +{ + SolidProtoHierarchyBase { + translationStep IS translationStep + rotationStep IS rotationStep + children IS children + ucField1 IS ucField1 + } +} diff --git a/tests/api/worlds/supervisor_proto.wbt b/tests/api/worlds/supervisor_proto.wbt new file mode 100644 index 00000000000..e10a7c1dade --- /dev/null +++ b/tests/api/worlds/supervisor_proto.wbt @@ -0,0 +1,45 @@ +#VRML_SIM R2024a utf8 + +EXTERNPROTO "webots://tests/api/protos/SolidProtoHierarchy.proto" +EXTERNPROTO "webots://tests/default/protos/TestSuiteEmitter.proto" +EXTERNPROTO "webots://tests/default/protos/TestSuiteSupervisor.proto" + +WorldInfo { + coordinateSystem "NUE" +} +Viewpoint { + orientation 0.3346016217580861 0.9268563248699502 0.170232510894447 5.0632475 + position -1.4014692 0.82937332 0.66081321 +} +DEF BACKGROUND Background { + skyColor [ + 0.4 0.7 1 + ] +} +PointLight { + attenuation 0 0 1 + intensity 0.13 + location 0 0.3 0 +} +DEF HIERARCHY SolidProtoHierarchy { +} +DEF Test Robot { + children [ + TestSuiteEmitter { + } + ] + name "supervisor_proto" + controller "supervisor_proto" + supervisor TRUE +} +DEF Test Robot { + children [ + TestSuiteEmitter { + } + ] + name "supervisor_proto_fields" + controller "supervisor_proto_fields" + supervisor TRUE +} +TestSuiteSupervisor { +} diff --git a/tests/lib/ts_assertion.h b/tests/lib/ts_assertion.h index dd22025f1fa..b0ec8976f02 100644 --- a/tests/lib/ts_assertion.h +++ b/tests/lib/ts_assertion.h @@ -30,12 +30,12 @@ void ts_assert_boolean_not_equal(bool value, const char *error_message, ...) { TS_FINAL_CHECK(); } -void ts_assert_pointer_null(void *ptr, const char *error_message, ...) { +void ts_assert_pointer_null(const void *ptr, const char *error_message, ...) { bool correct = ptr == NULL; TS_FINAL_CHECK(); } -void ts_assert_pointer_not_null(void *ptr, const char *error_message, ...) { +void ts_assert_pointer_not_null(const void *ptr, const char *error_message, ...) { bool correct = ptr != NULL; TS_FINAL_CHECK(); } diff --git a/tests/protos/controllers/template_indirect_field_access/template_indirect_field_access.c b/tests/protos/controllers/template_indirect_field_access/template_indirect_field_access.c index 6a5605d5473..75f6a19eb55 100644 --- a/tests/protos/controllers/template_indirect_field_access/template_indirect_field_access.c +++ b/tests/protos/controllers/template_indirect_field_access/template_indirect_field_access.c @@ -10,7 +10,7 @@ int main(int argc, char **argv) { ts_setup(argv[1]); // give the controller args WbNodeRef proto = wb_supervisor_node_get_from_def("PROTO_template_indirect_field_access"); - WbFieldRef radarCrossSection = wb_supervisor_node_get_proto_field(proto, "radarCrossSection"); + WbFieldRef radarCrossSection = wb_supervisor_node_get_base_node_field(proto, "radarCrossSection"); ts_assert_double_in_delta(wb_supervisor_field_get_sf_float(radarCrossSection), 6.0, 0.0001, "radarCrossSection should be 6.0"); ts_send_success();