From 3a7419f0a4a9baac59cac46bf24e6ec7747f3765 Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Mon, 21 Oct 2024 04:11:58 +0000 Subject: [PATCH] Sync submodule --- .../include/controller/c/webots/supervisor.h | 15 +- .../include/controller/c/webots/types.h | 1 + .../include/controller/cpp/webots/Field.hpp | 1 + .../include/controller/cpp/webots/Node.hpp | 9 +- .../include/controller/cpp/webots/Proto.hpp | 46 ++ .../controller/python/controller/__init__.py | 3 +- .../lib/controller/python/controller/field.py | 22 +- .../lib/controller/python/controller/node.py | 39 +- .../lib/controller/python/controller/proto.py | 60 +++ .../webots/src/controller/c/Controller.def | 16 +- .../webots/src/controller/c/messages.h | 1 + .../webots/src/controller/c/robot.c | 42 +- .../webots/src/controller/c/supervisor.c | 456 ++++++++++++++++-- .../webots/src/controller/cpp/Field.cpp | 5 + .../webots/src/controller/cpp/Makefile | 1 + .../webots/src/controller/cpp/Node.cpp | 17 +- .../webots/src/controller/cpp/Proto.cpp | 73 +++ .../webots/src/controller/cpp/Supervisor.cpp | 1 + .../webots/src/controller/java/Makefile | 1 + .../webots/src/controller/java/controller.i | 89 ++++ .../webots/src/controller/matlab/mgenerate.py | 18 +- 21 files changed, 826 insertions(+), 90 deletions(-) create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Proto.hpp create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/proto.py create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Proto.cpp diff --git a/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h b/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h index c2b71a6e1..ccb01222b 100644 --- a/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/include/controller/c/webots/types.h b/webots_ros2_driver/webots/include/controller/c/webots/types.h index 49a5fe7a7..08138b18e 100644 --- a/webots_ros2_driver/webots/include/controller/c/webots/types.h +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp index 1e00ebc58..3d97e2c43 100644 --- a/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp index a18ba417b..32c91f471 100644 --- a/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/include/controller/cpp/webots/Proto.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Proto.hpp new file mode 100644 index 000000000..8857afb9c --- /dev/null +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py b/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py index 61373467d..20509d5dd 100644 --- a/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/lib/controller/python/controller/field.py b/webots_ros2_driver/webots/lib/controller/python/controller/field.py index 3121cebea..0ebbfd90f 100644 --- a/webots_ros2_driver/webots/lib/controller/python/controller/field.py +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/lib/controller/python/controller/node.py b/webots_ros2_driver/webots/lib/controller/python/controller/node.py index a7c92ccf1..f47470815 100644 --- a/webots_ros2_driver/webots/lib/controller/python/controller/node.py +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/lib/controller/python/controller/proto.py b/webots_ros2_driver/webots/lib/controller/python/controller/proto.py new file mode 100644 index 000000000..d1a8a245b --- /dev/null +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/c/Controller.def b/webots_ros2_driver/webots/src/controller/c/Controller.def index 4d895a30a..d7fc80bbe 100644 --- a/webots_ros2_driver/webots/src/controller/c/Controller.def +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/c/messages.h b/webots_ros2_driver/webots/src/controller/c/messages.h index dad5d67a3..71eb3630e 100644 --- a/webots_ros2_driver/webots/src/controller/c/messages.h +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/c/robot.c b/webots_ros2_driver/webots/src/controller/c/robot.c index 8b32ca8c1..50616de66 100644 --- a/webots_ros2_driver/webots/src/controller/c/robot.c +++ b/webots_ros2_driver/webots/src/controller/c/robot.c @@ -1081,21 +1081,33 @@ static void wb_robot_cleanup_devices() { } } -static char *encode_robot_name(const char *robot_name) { +static char *encode_robot_name(const char *robot_name, int chars_used) { if (!robot_name) return NULL; char *encoded_name = percent_encode(robot_name); int length = strlen(encoded_name); // the robot name is used to connect to the libController and in this process there are indirect - // limitations such as QLocalServer only accepting strings up to 106 characters for server names, - // for these reasons if the robot name is bigger than an arbitrary length, a hashed version is used instead - if (length > 70) { // note: this threshold should be the same as in WbRobot.cpp + // limitations such as QLocalServer only accepting strings up to 91 characters long for server names + // on some platforms. + // Since the server name also contains the tmp path and that includes the user's username and, + // in the case of a Snap, the user's home directory, we need to limit the length of the encoded + // robot name based on the tmp path. If it is longer than that, then we compute a hashed version + // of the name and use as much of it as we can. If that would be less than 4 chars, we try to use + // 4 chars and hope we are on a platform where QLocalServer accepts longer names. 4 chars makes the + // chance of a name collision 1/65536. + // Note: It is critical that the same logic is used in WbRobot.cpp + int max_name_length = 91 - chars_used; + if (max_name_length < 4) + max_name_length = 4; + // Round down to the next multiple of 2 because it makes the code easier. + max_name_length = max_name_length / 2 * 2; + if (length > max_name_length) { char hash[21]; - char *output = malloc(41); + char *output = malloc(max_name_length + 1); SHA1(hash, encoded_name, length); free(encoded_name); - for (size_t i = 0; i < 20; i++) + for (size_t i = 0; i < max_name_length / 2 && i < 20; i++) sprintf((output + (2 * i)), "%02x", hash[i] & 0xff); return output; } @@ -1104,10 +1116,12 @@ static char *encode_robot_name(const char *robot_name) { } static char *compute_socket_filename(char *error_buffer) { - char *robot_name = encode_robot_name(wbu_system_getenv("WEBOTS_ROBOT_NAME")); const char *WEBOTS_INSTANCE_PATH = wbu_system_webots_instance_path(true); + char *robot_name = NULL; + const char *WEBOTS_ROBOT_NAME = wbu_system_getenv("WEBOTS_ROBOT_NAME"); char *socket_filename; - if (robot_name && robot_name[0] && WEBOTS_INSTANCE_PATH && WEBOTS_INSTANCE_PATH[0]) { + if (WEBOTS_ROBOT_NAME && WEBOTS_ROBOT_NAME[0] && WEBOTS_INSTANCE_PATH && WEBOTS_INSTANCE_PATH[0]) { + robot_name = encode_robot_name(WEBOTS_ROBOT_NAME, strlen(WEBOTS_INSTANCE_PATH) + strlen("ipc//intern")); #ifndef _WIN32 const int length = strlen(WEBOTS_INSTANCE_PATH) + strlen(robot_name) + 15; // "%sintern/%s/socket" socket_filename = malloc(length); @@ -1267,7 +1281,7 @@ static char *compute_socket_filename(char *error_buffer) { free(robot_name); const char *sub_string = strstr(&WEBOTS_CONTROLLER_URL[6], "/"); - robot_name = encode_robot_name(sub_string ? sub_string + 1 : NULL); + robot_name = encode_robot_name(sub_string ? sub_string + 1 : NULL, strlen(ipc_folder) + strlen("//extern")); if (robot_name) { #ifndef _WIN32 // socket file name is like: folder + robot_name + "/extern" @@ -1370,7 +1384,7 @@ static void compute_remote_info(char **host, int *port, char **robot_name) { snprintf(*host, host_length, "%s", &WEBOTS_CONTROLLER_URL[6]); sscanf(url_suffix, ":%d", port); const char *rn = strstr(url_suffix, "/"); - *robot_name = rn != NULL ? encode_robot_name(rn + 1) : NULL; + *robot_name = rn != NULL ? encode_robot_name(rn + 1, 0) : NULL; } int wb_robot_init() { // API initialization @@ -1462,11 +1476,11 @@ int wb_robot_init() { // API initialization retry -= 5; fprintf(stderr, "%s, pending until loading is done...\n", error_message); } else if (socket_filename) { + fprintf(stderr, + "The specified robot (at %s) is not in the list of robots with controllers, retrying for another %d " + "seconds...\n", + socket_filename, 50 - retry); free(socket_filename); - fprintf( - stderr, - "The specified robot is not in the list of robots with controllers, retrying for another %d seconds...\n", - 50 - retry); } else fprintf(stderr, "%s, retrying for another %d seconds...\n", error_message, 50 - retry); } diff --git a/webots_ros2_driver/webots/src/controller/c/supervisor.c b/webots_ros2_driver/webots/src/controller/c/supervisor.c index b20574914..0dfb5d4d5 100644 --- a/webots_ros2_driver/webots/src/controller/c/supervisor.c +++ b/webots_ros2_driver/webots/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; @@ -2831,6 +2983,9 @@ const double *wb_supervisor_virtual_reality_headset_get_orientation() { } const char *wb_supervisor_field_get_name(WbFieldRef field) { + if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false)) + return ""; + return field->name; } @@ -2842,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; @@ -2851,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__); @@ -2922,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; @@ -2941,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; @@ -3030,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; @@ -3038,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; @@ -3046,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; @@ -3054,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; @@ -3062,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; @@ -3070,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; @@ -3078,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; @@ -3086,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 ""; @@ -3094,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; @@ -3108,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; @@ -3116,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; @@ -3124,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; @@ -3132,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; @@ -3140,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; @@ -3148,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; @@ -3156,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; @@ -3164,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 ""; @@ -3172,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; @@ -3183,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; @@ -3672,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/webots_ros2_driver/webots/src/controller/cpp/Field.cpp b/webots_ros2_driver/webots/src/controller/cpp/Field.cpp index 529cfdb46..f96576691 100644 --- a/webots_ros2_driver/webots/src/controller/cpp/Field.cpp +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/cpp/Makefile b/webots_ros2_driver/webots/src/controller/cpp/Makefile index 89ccd7da9..496fcc0a7 100644 --- a/webots_ros2_driver/webots/src/controller/cpp/Makefile +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/cpp/Node.cpp b/webots_ros2_driver/webots/src/controller/cpp/Node.cpp index c8a3cce72..613b108d6 100644 --- a/webots_ros2_driver/webots/src/controller/cpp/Node.cpp +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/cpp/Proto.cpp b/webots_ros2_driver/webots/src/controller/cpp/Proto.cpp new file mode 100644 index 000000000..bb722e00e --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/cpp/Proto.cpp @@ -0,0 +1,73 @@ +// Copyright 1996-2023 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// https://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#define WB_ALLOW_MIXING_C_AND_CPP_API +#include +#include + +#include +#include + +using namespace std; +using namespace webots; + +static map protoMap; + +Proto *Proto::findProto(WbProtoRef ref) { + if (!ref) + return NULL; + + map::iterator iter = protoMap.find(ref); + if (iter != protoMap.end()) + return iter->second; + + Proto *proto = new Proto(ref); + protoMap.insert(pair(ref, proto)); + return proto; +} + +void Proto::cleanup() { + map::iterator iter; + for (iter = protoMap.begin(); iter != protoMap.end(); ++iter) + delete (*iter).second; + + protoMap.clear(); +} + +Proto::Proto(WbProtoRef ref) : protoRef(ref) { +} + +string Proto::getTypeName() const { + return wb_supervisor_proto_get_type_name(protoRef); +} + +bool Proto::isDerived() const { + return wb_supervisor_proto_is_derived(protoRef); +} + +Proto *Proto::getParent() const { + return findProto(wb_supervisor_proto_get_parent(protoRef)); +} + +Field *Proto::getField(const string &fieldName) const { + return Field::findField(wb_supervisor_proto_get_field(protoRef, fieldName.c_str())); +} + +Field *Proto::getFieldByIndex(const int index) const { + return Field::findField(wb_supervisor_proto_get_field_by_index(protoRef, index)); +} + +int Proto::getNumberOfFields() const { + return wb_supervisor_proto_get_number_of_fields(protoRef); +} diff --git a/webots_ros2_driver/webots/src/controller/cpp/Supervisor.cpp b/webots_ros2_driver/webots/src/controller/cpp/Supervisor.cpp index f478974df..106bdc3ec 100644 --- a/webots_ros2_driver/webots/src/controller/cpp/Supervisor.cpp +++ b/webots_ros2_driver/webots/src/controller/cpp/Supervisor.cpp @@ -23,6 +23,7 @@ using namespace webots; Supervisor::~Supervisor() { Field::cleanup(); Node::cleanup(); + Proto::cleanup(); } Supervisor *Supervisor::getSupervisorInstance() { diff --git a/webots_ros2_driver/webots/src/controller/java/Makefile b/webots_ros2_driver/webots/src/controller/java/Makefile index 8f1057def..e89e79887 100644 --- a/webots_ros2_driver/webots/src/controller/java/Makefile +++ b/webots_ros2_driver/webots/src/controller/java/Makefile @@ -70,6 +70,7 @@ JAVA_FILES = Accelerometer.java \ Pen.java \ PositionSensor.java \ Propeller.java \ + Proto.java \ Radar.java \ RadarTarget.java \ RangeFinder.java \ diff --git a/webots_ros2_driver/webots/src/controller/java/controller.i b/webots_ros2_driver/webots/src/controller/java/controller.i index e62517837..b72ed0e0a 100644 --- a/webots_ros2_driver/webots/src/controller/java/controller.i +++ b/webots_ros2_driver/webots/src/controller/java/controller.i @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -436,12 +437,20 @@ namespace webots { %ignore webots::Field::findField(WbFieldRef ref); %ignore webots::Field::cleanup(); +%rename("getActualFieldPrivate") getActualField() const; +%javamethodmodifiers getActualField() const "private" + %rename("getSFNodePrivate") getSFNode() const; %rename("getMFNodePrivate") getMFNode(int index) const; %javamethodmodifiers getSFNode() const "private" %javamethodmodifiers getMFNode(int index) const "private" %typemap(javacode) webots::Field %{ + public Field getActualField() { + long cPtr = wrapperJNI.Field_getActualFieldPrivate(swigCPtr, this); + return Field.findField(cPtr); + } + public Node getSFNode() { long cPtr = wrapperJNI.Field_getSFNodePrivate(swigCPtr, this); return Node.findNode(cPtr); @@ -632,11 +641,20 @@ namespace webots { %rename("getParentNodePrivate") getParentNode() const; %javamethodmodifiers getParentNode() const "private" +%rename("getProtoPrivate") getProto() const; +%javamethodmodifiers getProto() const "private" + %rename("getFromProtoDefPrivate") getFromProtoDef(const std::string &name) const; %javamethodmodifiers getFromProtoDef(const std::string &name) const "private" %rename("getFieldPrivate") getField(const std::string &fieldName) const; +%rename("getFieldByIndexPrivate") getFieldByIndex(const int index) const; +%rename("getBaseNodeFieldPrivate") getBaseNodeField(const std::string &fieldName) const; +%rename("getBaseNodeFieldByIndexPrivate") getBaseNodeFieldByIndex(const int index) const; %javamethodmodifiers getField(const std::string &fieldName) const "private" +%javamethodmodifiers getFieldByIndex(const int index) const "private" +%javamethodmodifiers getBaseNodeField(const std::string &fieldName) const "private" +%javamethodmodifiers getBaseNodeFieldByIndex(const int index) const "private" %apply int *OUTPUT { int *size }; %rename(getContactPointsPrivate) getContactPoints; @@ -672,6 +690,11 @@ namespace webots { return Node.findNode(cPtr); } + public Proto getProto() { + long cPtr = wrapperJNI.Node_getProtoPrivate(swigCPtr, this); + return Proto.findProto(cPtr); + } + public Node getFromProtoDef(String name) { long cPtr = wrapperJNI.Node_getFromProtoDefPrivate(swigCPtr, this, name); return Node.findNode(cPtr); @@ -682,6 +705,21 @@ namespace webots { return Field.findField(cPtr); } + public Field getFieldByIndex(int index) { + long cPtr = wrapperJNI.Node_getFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + + public Field getBaseNodeField(String fieldName) { + long cPtr = wrapperJNI.Node_getBaseNodeFieldPrivate(swigCPtr, this, fieldName); + return Field.findField(cPtr); + } + + public Field getBaseNodeFieldByIndex(int index) { + long cPtr = wrapperJNI.Node_getBaseNodeFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + private static java.util.HashMap nodes = new java.util.HashMap(); // DO NOT USE THIS FUNCTION: IT IS RESERVED FOR INTERNAL USE ! @@ -733,6 +771,57 @@ namespace webots { %} %include +//---------------------------------------------------------------------------------------------- +// Proto +//---------------------------------------------------------------------------------------------- + +%ignore webots::Proto::findProto(WbProtoRef ref); +%ignore webots::Proto::cleanup(); + +%rename("getParentPrivate") getParent() const; +%javamethodmodifiers getParent() const "private" + +%rename("getFieldPrivate") getField(const std::string &fieldName) const; +%rename("getFieldByIndexPrivate") getFieldByIndex(const int index) const; +%javamethodmodifiers getField(const std::string &fieldName) const "private" +%javamethodmodifiers getFieldByIndex(const int index) const "private" + +%typemap(javacode) webots::Proto %{ +// ----- begin hand written section ---- + public Proto getParent() { + long cPtr = wrapperJNI.Proto_getParentPrivate(swigCPtr, this); + return Proto.findProto(cPtr); + } + + public Field getField(String fieldName) { + long cPtr = wrapperJNI.Proto_getFieldPrivate(swigCPtr, this, fieldName); + return Field.findField(cPtr); + } + + public Field getFieldByIndex(int index) { + long cPtr = wrapperJNI.Proto_getFieldByIndexPrivate(swigCPtr, this, index); + return Field.findField(cPtr); + } + + private static java.util.HashMap protos = new java.util.HashMap(); + + // DO NOT USE THIS FUNCTION: IT IS RESERVED FOR INTERNAL USE ! + public static Proto findProto(long cPtr) { + if (cPtr == 0) + return null; + + Proto proto = protos.get(new Long(cPtr)); + if (proto != null) + return proto; + + proto = new Proto(cPtr, false); + protos.put(new Long(cPtr), proto); + return proto; + } +%} + +%include + //---------------------------------------------------------------------------------------------- // Radar //---------------------------------------------------------------------------------------------- diff --git a/webots_ros2_driver/webots/src/controller/matlab/mgenerate.py b/webots_ros2_driver/webots/src/controller/matlab/mgenerate.py index 4d2a94f9a..3e27f5b3b 100755 --- a/webots_ros2_driver/webots/src/controller/matlab/mgenerate.py +++ b/webots_ros2_driver/webots/src/controller/matlab/mgenerate.py @@ -445,6 +445,7 @@ def main(args=None): generator.gen(PROC, "wb_supervisor_export_image(filename, quality)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_disable_sf_tracking(field)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_enable_sf_tracking(field, sampling_period)", "supervisor") + generator.gen(FUNC, "wb_supervisor_field_get_actual_field(fieldref)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_get_count(fieldref)", "supervisor") generator.gen(FUNC, "wb_supervisor_field_get_mf_bool(fieldref, index)", "supervisor") # generator.gen(FUNC, "wb_supervisor_field_get_mf_color(fieldref, index)", "supervisor") @@ -517,6 +518,8 @@ def main(args=None): "supervisor") # DEPRECATED generator.gen(FUNC, "wb_supervisor_node_enable_pose_tracking(sampling_period, node, from_node)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_export_string(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_base_node_field(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_base_node_field_by_index(noderef, fieldindex)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_base_type_name(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_center_of_mass(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_contact_point(noderef, index)", "supervisor") @@ -524,21 +527,20 @@ def main(args=None): # generator.gen(PROC, "wb_supervisor_node_get_contact_points(noderef, include_descendants)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_def(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_field(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_field_by_index(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_field_by_index(noderef, fieldindex)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_def(defname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_device(tag)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_id(id)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_from_proto_def(noderef, defname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_id(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_number_of_base_node_fields(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_number_of_contact_points(noderef, include_descendants)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_number_of_fields(noderef, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_number_of_fields(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_orientation(noderef)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_parent_node(noderef)", "supervisor") + generator.gen(FUNC, "wb_supervisor_node_get_proto(noderef)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_pose(noderef, noderef_from)", "supervisor") # generator.gen(FUNC, "wb_supervisor_node_get_position(noderef)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_field(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_field_by_index(noderef, fieldname)", "supervisor") - generator.gen(FUNC, "wb_supervisor_node_get_proto_number_of_fields(noderef, fieldname)", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_root()", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_selected()", "supervisor") generator.gen(FUNC, "wb_supervisor_node_get_self()", "supervisor") @@ -556,6 +558,12 @@ def main(args=None): generator.gen(FUNC, "wb_supervisor_node_set_joint_position(noderef, position, index)", "supervisor") generator.gen(PROC, "wb_supervisor_node_set_velocity(noderef, velocity)", "supervisor") generator.gen(PROC, "wb_supervisor_node_set_visibility(node, from, visible)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_field(protoref, fieldname)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_field_by_index(protoref, fieldindex)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_number_of_fields(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_parent(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_get_type_name(protoref)", "supervisor") + generator.gen(FUNC, "wb_supervisor_proto_is_derived(protoref)", "supervisor") # generator.gen(FUNC, "wb_supervisor_save_world(filename)", "supervisor"); # DEPRECATED # generator.gen(PROC, "wb_supervisor_set_label()", "supervisor") generator.gen(FUNC, "wb_supervisor_simulation_get_mode()", "supervisor")