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