Fixes supervisor node python API (cyberbotics#5633)

* Update node.py

* Update changelog-r2023.md

* clean-up

* Update changelog-r2023.md

* Fixed more references to deprecated API function

* clang-format

* Update wb_supervisor_node_enable_contact_points_tracking.m

* Update supervisor.c

* PEP8

* Update changelog-r2023.md

* Update changelog-r2023.md

* Fixed return value of field getters in case a field is not existing and moved VR functions in the supevisor.py file

* Fixed pointless argument to wb_supervisor_node_disable_contact_points_tracking

* MATLAB function

* Removed deprecation from R2023a-rev1

* More changelog information

* Fixed MATLAB generator

* webots_ros

* Update src/controller/cpp/Node.cpp

Co-authored-by: Yannick Goumaz <[email protected]>

* Removed duplicate functions

Co-authored-by: Yannick Goumaz <[email protected]>

docs/reference/changelog-r2023.md

@@ -8,6 +8,8 @@ Released on ??
     - Added `recognitionColors` field to [SolidBox](../guide/object-solids.md#solidbox) and Tractor PROTO models ([#5606](https://github.com/cyberbotics/webots/pull/5606)).
   - Bug Fixes
     - Fixed Python API `Supervisor.setSimulationMode` which was failing ([#5603](https://github.com/cyberbotics/webots/pull/5603)).
+    - Fixed Python API `Node.enableContactPointsTracking` which was failing ([#5633](https://github.com/cyberbotics/webots/pull/5633)).
+    - Fixed Python API field getters sometimes returning an invalid Field object ([#5633](https://github.com/cyberbotics/webots/pull/5633)).
     - Fixed Python API `Field.enableSFTracking` and `Field.disableSFTracking` which were failing ([#5640](https://github.com/cyberbotics/webots/pull/5640)).
     - Fixed crash resulting from requesting pose tracking of unsuitable nodes ([5620](https://github.com/cyberbotics/webots/pull/5620)).
 

docs/reference/robot.md

@@ -263,7 +263,7 @@ This includes some [Supervisor API](supervisor.md) functions, like `wb_superviso
 Webots will warn you in case you call one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`.
 You can simply call them before `wb_robot_step_begin` or after `wb_robot_step_end`.
 However, some of these functions can be called between `wb_robot_step_begin` and `wb_robot_step_end` if you enable the supervisor tracking feature.
-`wb_supervisor_field_enable_sf_tracking`, `wb_supervisor_node_enable_pose_tracking` and `wb_supervisor_node_enable_contact_point_tracking` force Webots to continuously stream the requested information to the controller.
+`wb_supervisor_field_enable_sf_tracking`, `wb_supervisor_node_enable_pose_tracking` and `wb_supervisor_node_enable_contact_points_tracking` force Webots to continuously stream the requested information to the controller.
 By enabling the tracking, the corresponding supervisor functions can be called between `wb_robot_step_begin` and `wb_robot_step_end`, because their value will be queried to Webots during `wb_robot_step_begin` and received during `wb_robot_step_end`.
 
 The C API has two additional functions: `wb_robot_init` and `wb_robot_cleanup`.

docs/reference/supervisor.md

@@ -1114,8 +1114,8 @@ The "[WEBOTS\_HOME/projects/samples/howto/center\_of\_mass/worlds/center\_of\_ma
 ---
 
 #### `wb_supervisor_node_get_contact_points`
-#### `wb_supervisor_node_enable_contact_point_tracking`
-#### `wb_supervisor_node_disable_contact_point_tracking`
+#### `wb_supervisor_node_enable_contact_points_tracking`
+#### `wb_supervisor_node_disable_contact_points_tracking`
 
 
 %tab-component "language"
@@ -1126,8 +1126,8 @@ The "[WEBOTS\_HOME/projects/samples/howto/center\_of\_mass/worlds/center\_of\_ma
 #include <webots/supervisor.h>
 
 WbContactPoint *wb_supervisor_node_get_contact_points(WbNodeRef node, bool include_descendants, int *size);
-wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants);
-wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants);
+wb_supervisor_node_enable_contact_points_tracking(WbNodeRef node, int sampling_period, bool include_descendants);
+wb_supervisor_node_disable_contact_points_tracking(WbNodeRef node);
 ```
 
 %tab-end
@@ -1141,7 +1141,7 @@ namespace webots {
   class Node {
     ContactPoint *getContactPoints(bool includeDescendants, int *size) const;
     void enableContactPointsTracking(int samplingPeriod, bool includeDescendants = false) const;
-    void disableContactPointsTracking(bool includeDescendants = false) const;
+    void disableContactPointsTracking() const;
     // ...
   }
 }
@@ -1157,7 +1157,7 @@ from controller import Node
 class Node:
     def getContactPoints(includeDescendants=False):
     def enableContactPointsTracking(samplingPeriod, includeDescendants=False):
-    def disableContactPointsTracking(includeDescendants=False):
+    def disableContactPointsTracking():
     # ...
 ```
 
@@ -1171,7 +1171,7 @@ import com.cyberbotics.webots.controller.Node;
 public class Node {
   public ContactPoint[] getContactPoints(boolean includeDescendants);
   public enableContactPointsTracking(int samplingPeriod, boolean includeDescendants = false);
-  public disableContactPointsTracking(boolean includeDescendants = false);
+  public disableContactPointsTracking();
   // ...
 }
 ```
@@ -1182,8 +1182,8 @@ public class Node {
 
 ```MATLAB
 contact_point = wb_supervisor_node_get_contact_points(include_descendants):
-wb_supervisor_node_enable_contact_point_tracking(sampling_period, include_descendants):
-wb_supervisor_node_disable_contact_point_tracking(include_descendants):
+wb_supervisor_node_enable_contact_points_tracking(sampling_period, include_descendants):
+wb_supervisor_node_disable_contact_points_tracking():
 ```
 
 %tab-end
@@ -1193,8 +1193,8 @@ wb_supervisor_node_disable_contact_point_tracking(include_descendants):
 | name | service/topic | data type | data type definition |
 | --- | --- | --- | --- |
 | `/supervisor/node/get_contact_points` | `service` | `webots_ros::node_get_contact_points` | `uint64 node`<br/>`---`<br/>[`webots_ros/ContactPoint[]`](supervisor.md#contact-point) contact_points |
-| `/supervisor/node/enable_contact_point_tracking` | `service` | `webots_ros::enable_contact_point_tracking` | `uint64 node`<br/>`int32 sampling_period`<br/>`bool include_descendants`<br/>`---`<br/>`int32 success` |
-| `/supervisor/node/disable_contact_points_tracking` | `service` | `webots_ros::disable_contact_points_tracking` | `uint64 node`<br/>`bool include_descendants`<br/>`---`<br/>`int32 success` |
+| `/supervisor/node/enable_contact_points_tracking` | `service` | `webots_ros::enable_contact_points_tracking` | `uint64 node`<br/>`int32 sampling_period`<br/>`bool include_descendants`<br/>`---`<br/>`int32 success` |
+| `/supervisor/node/disable_contact_points_tracking` | `service` | `webots_ros::disable_contact_points_tracking` | `uint64 node`<br/>`---`<br/>`int32 success` |
 
 %tab-end
 
@@ -1209,11 +1209,11 @@ The `include_descendants` argument defines whether the descendant nodes should a
 The descendant nodes are the nodes included within the node given as an argument.
 The `size` argument is an output argument and it returns a number of contact points in the list.
 
-The `wb_supervisor_node_enable_contact_point_tracking` function forces Webots to stream contact point data to the controller.
+The `wb_supervisor_node_enable_contact_points_tracking` function forces Webots to stream contact points 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 contact point data should be sent to the controller.
+The `sampling_period` argument determines how often the contact points data should be sent to the controller.
 
-The `wb_supervisor_node_disable_contact_point_tracking` function disables contact point data tracking.
+The `wb_supervisor_node_disable_contact_points_tracking` function disables contact points data tracking.
 
 The "[WEBOTS\_HOME/projects/samples/howto/cylinder\_stack/worlds/cylinder\_stack.wbt]({{ url.github_tree }}/projects/samples/howto/cylinder_stack/worlds/cylinder_stack.wbt)" project shows how to use this function.
 

include/controller/c/webots/supervisor.h

@@ -144,8 +144,9 @@ void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_perio
 void wb_supervisor_field_disable_sf_tracking(WbFieldRef field);
 void wb_supervisor_node_enable_pose_tracking(WbNodeRef node, int sampling_period, WbNodeRef from_node);
 void wb_supervisor_node_disable_pose_tracking(WbNodeRef node, WbNodeRef from_node);
-void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants);
-void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants);
+
+void wb_supervisor_node_enable_contact_points_tracking(WbNodeRef node, int sampling_period, bool include_descendants);
+void wb_supervisor_node_disable_contact_points_tracking(WbNodeRef node);
 
 bool wb_supervisor_field_get_sf_bool(WbFieldRef field);
 int wb_supervisor_field_get_sf_int32(WbFieldRef field);
@@ -203,6 +204,12 @@ const double *wb_supervisor_virtual_reality_headset_get_position();
 const double *wb_supervisor_virtual_reality_headset_get_orientation();
 
 // Deprecated functions
+
+// deprecated since Webots R2023a revision 1
+// use wb_supervisor_node_enable_contact_points_tracking or wb_supervisor_node_disable_contact_points_tracking instead
+void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants);
+void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants);
+
 // deprecated since Webots R2018b
 void wb_supervisor_simulation_revert() WB_DEPRECATED;               // please use wb_supervisor_world_reload() instead
 void wb_supervisor_load_world(const char *filename) WB_DEPRECATED;  // please use wb_supervisor_world_load() instead

include/controller/cpp/webots/Node.hpp

@@ -142,10 +142,8 @@ namespace webots {
     const double *getOrientation() const;
     const double *getPose() const;
     const double *getPose(const Node *fromNode) const;
-    void enableContactPointsTracking(int samplingPeriod) const;
-    void disableContactPointsTracking() const;
-    void enableContactPointsTracking(int samplingPeriod, bool includeDescendants) const;
-    void disableContactPointsTracking(bool includeDescendants) const;
+    void enableContactPointsTracking(int samplingPeriod, bool includeDescendants = false) const;
+    void disableContactPointsTracking(bool includeDescendants = false) const;
     void enablePoseTracking(int samplingPeriod) const;
     void disablePoseTracking() const;
     void enablePoseTracking(int samplingPeriod, const Node *fromNode) const;

lib/controller/matlab/.gitignore

@@ -295,8 +295,10 @@ wb_supervisor_node_add_force.m
 wb_supervisor_node_add_force_with_offset.m
 wb_supervisor_node_add_torque.m
 wb_supervisor_node_disable_contact_point_tracking.m
+wb_supervisor_node_disable_contact_points_tracking.m
 wb_supervisor_node_disable_pose_tracking.m
 wb_supervisor_node_enable_contact_point_tracking.m
+wb_supervisor_node_enable_contact_points_tracking.m
 wb_supervisor_node_enable_pose_tracking.m
 wb_supervisor_node_export_string.m
 wb_supervisor_node_get_base_type_name.m

lib/controller/matlab/wb_supervisor_node_disable_contact_points_tracking.m

@@ -0,0 +1,6 @@
+function result = wb_supervisor_node_disable_contact_points_tracking(noderef)
+% Usage: wb_supervisor_node_disable_contact_points_tracking(noderef)
+% Matlab API for Webots
+% Online documentation is available <a href="https://www.cyberbotics.com/doc/reference/supervisor">here</a>
+
+result = calllib('libController', 'wb_supervisor_node_disable_contact_points_tracking', noderef);

lib/controller/matlab/wb_supervisor_node_enable_contact_points_tracking.m

@@ -0,0 +1,6 @@
+function result = wb_supervisor_node_enable_contact_points_tracking(noderef, sampling_period, include_descendants)
+% Usage: wb_supervisor_node_enable_contact_points_tracking(noderef, sampling_period, include_descendants)
+% Matlab API for Webots
+% Online documentation is available <a href="https://www.cyberbotics.com/doc/reference/supervisor">here</a>
+
+result = calllib('libController', 'wb_supervisor_node_enable_contact_points_tracking', noderef, sampling_period, include_descendants);

lib/controller/python/controller/field.py

@@ -241,15 +241,6 @@ def importMFNodeFromString(self, position: int, nodeString: str):
     def importSFNodeFromString(self, nodeString: str):
         wb.wb_supervisor_field_import_sf_node_from_string(self._ref, str.encode(nodeString))
 
-    def virtualRealityHeadsetIsUsed(self):
-        return wb.wb_supervisor_virtual_reality_headset_is_used() != 0
-
-    def virtualRealityHeadsetGetPosition(self) -> typing.List[float]:
-        return wb.wb_supervisor_virtual_reality_headset_get_position()
-
-    def virtualRealityHeadsetGetOrientation(self) -> typing.List[float]:
-        return wb.wb_supervisor_virtual_reality_headset_get_orientation()
-
     @property
     def name(self) -> str:
         return wb.wb_supervisor_field_get_name(self._ref).decode()

lib/controller/python/controller/node.py

@@ -110,19 +110,23 @@ def exportString(self):
         return wb.wb_supervisor_node_export_string(self._ref).decode()
 
     def getField(self, name: str) -> Field:
-        return Field(self, name=name)
+        field = Field(self, name=name)
+        return field if field._ref else None
 
     def getFieldByIndex(self, index: int) -> Field:
-        return Field(self, index=index)
+        field = Field(self, index=index)
+        return field if field._ref else None
 
     def getNumberOfFields(self) -> int:
         return self.number_of_fields
 
     def getProtoField(self, name: str) -> Field:
-        return Field(self, name=name, proto=True)
+        field = Field(self, name=name, proto=True)
+        return field if field._ref else None
 
     def getProtoFieldByIndex(self, index: int) -> Field:
-        return Field(self, index=index, proto=True)
+        field = Field(self, index=index, proto=True)
+        return field if field._ref else None
 
     def getPosition(self) -> typing.List[float]:
         p = wb.wb_supervisor_node_get_position(self._ref)
@@ -158,11 +162,12 @@ def getContactPoints(self, includeDescendants: bool = False) -> typing.List[Cont
             contact_points.append(ContactPoint(struct.unpack_from('3di', points, 28 * i)))
         return contact_points
 
-    def enableContactPointTracking(self, samplingPeriod: int, includeDescendants: bool = False):
-        wb.wb_supervisor_node_enable_contact_point_tracking(samplingPeriod, 1 if includeDescendants else 0)
+    def enableContactPointsTracking(self, samplingPeriod: int, includeDescendants: bool = False):
+        wb.wb_supervisor_node_enable_contact_points_tracking(self._ref, samplingPeriod, 1 if includeDescendants else 0)
 
-    def disableContactPointTracking(self, includeDescendants: bool = False):
-        wb.wb_supervisor_node_disable_contact_point_tracking(1 if includeDescendants else 0)
+    def disableContactPointsTracking(self, includeDescendants: bool = False):
+        # includeDescendants is kept for backwards compatibility, but should not be used in new code
+        wb.wb_supervisor_node_disable_contact_points_tracking(self._ref)
 
     def getStaticBalance(self) -> bool:
         return wb.wb_supervisor_node_get_static_balance(self._ref) != 0

lib/controller/python/controller/supervisor.py

@@ -16,6 +16,7 @@
 from .node import Node
 from .robot import Robot
 import ctypes
+import typing
 
 
 class Supervisor(Robot):
@@ -97,6 +98,15 @@ def animationStartRecording(self, filename: str):
     def animationStopRecording(self):
         return wb.wb_supervisor_animation_stop_recording()
 
+    def virtualRealityHeadsetIsUsed(self):
+        return wb.wb_supervisor_virtual_reality_headset_is_used() != 0
+
+    def virtualRealityHeadsetGetPosition(self) -> typing.List[float]:
+        return wb.wb_supervisor_virtual_reality_headset_get_position()
+
+    def virtualRealityHeadsetGetOrientation(self) -> typing.List[float]:
+        return wb.wb_supervisor_virtual_reality_headset_get_orientation()
+
     @property
     def simulation_mode(self) -> int:
         return wb.wb_supervisor_simulation_get_mode()

projects/default/controllers/ros/RosSupervisor.cpp

@@ -72,7 +72,7 @@ RosSupervisor::RosSupervisor(Ros *ros, Supervisor *supervisor) {
   mNodeGetContactPointsServer = mRos->nodeHandle()->advertiseService("supervisor/node/get_contact_points",
                                                                      &RosSupervisor::nodeGetContactPointsCallback, this);
   mNodeEnableContactPointsTrackingServer = mRos->nodeHandle()->advertiseService(
-    "supervisor/node/enable_contact_point_tracking", &RosSupervisor::nodeEnableContactPointsTrackingCallback, this);
+    "supervisor/node/enable_contact_points_tracking", &RosSupervisor::nodeEnableContactPointsTrackingCallback, this);
   mNodeDisableContactPointsTrackingServer = mRos->nodeHandle()->advertiseService(
     "supervisor/node/disable_contact_points_tracking", &RosSupervisor::nodeDisableContactPointsTrackingCallback, this);
 

src/controller/c/Controller.def

@@ -517,8 +517,10 @@ wb_supervisor_movie_start_recording
 wb_supervisor_movie_stop_recording
 wb_supervisor_movie_get_status
 wb_supervisor_node_disable_contact_point_tracking
+wb_supervisor_node_disable_contact_points_tracking
 wb_supervisor_node_disable_pose_tracking
 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_type_name

src/controller/c/supervisor.c

@@ -2848,7 +2848,7 @@ int wb_supervisor_field_get_count(WbFieldRef field) {
   return ((WbFieldStruct *)field)->count;
 }
 
-void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants) {
+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__);
     return;
@@ -2877,7 +2877,20 @@ void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampli
   robot_mutex_unlock();
 }
 
-void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants) {
+// to be officially deprecated in R2023b
+void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants) {
+  /*
+  static bool deprecation_warning = true;
+  if (deprecation_warning) {
+    fprintf(stderr, "Warning: %s() is deprecated, use wb_supervisor_node_enable_contact_points_tracking() instead.\n",
+            __FUNCTION__);
+    deprecation_warning = false;
+  }
+  */
+  wb_supervisor_node_enable_contact_points_tracking(node, sampling_period, include_descendants);
+}
+
+void wb_supervisor_node_disable_contact_points_tracking(WbNodeRef node) {
   if (!robot_check_supervisor(__FUNCTION__))
     return;
 
@@ -2890,12 +2903,25 @@ void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool incl
   contact_point_change_tracking_requested = true;
   contact_point_change_tracking.node = node;
   contact_point_change_tracking.enable = false;
-  contact_point_change_tracking.include_descendants = include_descendants;
+  contact_point_change_tracking.include_descendants = false;
   wb_robot_flush_unlocked(__FUNCTION__);
   pose_change_tracking_requested = false;
   robot_mutex_unlock();
 }
 
+// to be officially deprecated in R2023b
+void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants) {
+  /*
+  static bool deprecation_warning = true;
+  if (deprecation_warning) {
+    fprintf(stderr, "Warning: %s() is deprecated, use wb_supervisor_node_disable_contact_points_tracking() instead.\n",
+            __FUNCTION__);
+    deprecation_warning = false;
+  }
+  */
+  wb_supervisor_node_disable_contact_points_tracking(node);
+}
+
 void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_period) {
   if (!check_field(field, __FUNCTION__, WB_NO_FIELD, false, NULL, false, false))
     return;

src/controller/cpp/Node.cpp

@@ -131,20 +131,12 @@ const double *Node::getPose(const Node *fromNode) const {
   return wb_supervisor_node_get_pose(nodeRef, fromNode->nodeRef);
 }
 
-void Node::enableContactPointsTracking(int samplingPeriod) const {
-  wb_supervisor_node_enable_contact_point_tracking(nodeRef, samplingPeriod, false);
-}
-
-void Node::disableContactPointsTracking() const {
-  wb_supervisor_node_disable_contact_point_tracking(nodeRef, false);
-}
-
 void Node::enableContactPointsTracking(int samplingPeriod, bool includeDescendants) const {
-  wb_supervisor_node_enable_contact_point_tracking(nodeRef, samplingPeriod, false);
+  wb_supervisor_node_enable_contact_points_tracking(nodeRef, samplingPeriod, false);
 }
 
 void Node::disableContactPointsTracking(bool includeDescendants) const {
-  wb_supervisor_node_disable_contact_point_tracking(nodeRef, false);
+  wb_supervisor_node_disable_contact_points_tracking(nodeRef);
 }
 
 ContactPoint *Node::getContactPoints(bool includeDescendants, int *size) const {