Add wb_supervisor_get_from_device function

docs/reference/changelog-r2021.md

@@ -0,0 +1,7 @@
+# Webots R2021 Change Log
+
+## Webots R2021a
+Released on December XXth, 2020.
+
+  - New Features
+    - Added the `wb_supervisor_node_get_from_device` function to retrieve the node's handle of a device ([#2074](https://github.com/cyberbotics/webots/pull/2074)).

docs/reference/changelog.md

@@ -2,6 +2,7 @@
 
 ## Versions
 
+- [Webots R2021](changelog-r2021.md)
 - [Webots R2020](changelog-r2020.md)
 - [Webots R2019](changelog-r2019.md)
 - [Webots R2018](changelog-r2018.md)

docs/reference/device.md

@@ -9,7 +9,7 @@ Device {
 
 ### Description
 
-This abstract node (not instanciable) represents a robot device (actuator and/or sensor).
+This abstract node (that cannot be instantiated) represents a robot device (actuator and/or sensor).
 
 ### Device Functions
 

docs/reference/menu.md

@@ -123,6 +123,7 @@
 - [Other APIs](other-apis.md)
     - [ROS API](ros-api.md)
 - [Changelog](changelog.md)
+    - [Webots R2021](changelog-r2021.md)
     - [Webots R2020](changelog-r2020.md)
     - [Webots R2019](changelog-r2019.md)
     - [Webots R2018](changelog-r2018.md)

docs/reference/supervisor.md

@@ -16,6 +16,7 @@ As for a regular [Robot](robot.md) controller, the `wb_robot_init`, `wb_robot_st
 #### `wb_supervisor_node_get_self`
 #### `wb_supervisor_node_get_from_def`
 #### `wb_supervisor_node_get_from_id`
+#### `wb_supervisor_node_get_from_device`
 #### `wb_supervisor_node_get_selected`
 
 %tab-component "language"
@@ -29,6 +30,7 @@ WbNodeRef wb_supervisor_node_get_root();
 WbNodeRef wb_supervisor_node_get_self();
 WbNodeRef wb_supervisor_node_get_from_def(const char *def);
 WbNodeRef wb_supervisor_node_get_from_id(int id);
+WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag);
 WbNodeRef wb_supervisor_node_get_selected();
 ```
 
@@ -45,6 +47,7 @@ namespace webots {
     Node *getSelf();
     Node *getFromDef(const std::string &name);
     Node *getFromId(int id);
+    Node *getFromDevice(const Device *device);
     Node *getSelected();
     // ...
   }
@@ -63,6 +66,7 @@ class Supervisor (Robot):
     def getSelf(self):
     def getFromDef(self, name):
     def getFromId(self, id):
+    def getFromDevice(self, device);
     def getSelected(self):
     # ...
 ```
@@ -79,6 +83,7 @@ public class Supervisor extends Robot {
   public Node getSelf();
   public Node getFromDef(String name);
   public Node getFromId(int id);
+  public Node getFromDevice(Device device);
   public Node getSelected();
   // ...
 }
@@ -93,6 +98,7 @@ node = wb_supervisor_node_get_root()
 node = wb_supervisor_node_get_self()
 node = wb_supervisor_node_get_from_def('def')
 node = wb_supervisor_node_get_from_id(id)
+node = wb_supervisor_node_get_from_device(tag)
 node = wb_supervisor_node_get_selected()
 ```
 
@@ -106,6 +112,7 @@ node = wb_supervisor_node_get_selected()
 | `/supervisor/get_self` | `service` | [`webots_ros::get_uint64`](ros-api.md#common-services) | |
 | `/supervisor/get_from_def` | `service` | `webots_ros::supervisor_get_from_def` | `string name`<br/>`uint64 proto`<br/>`---`<br/>`uint64 node` |
 | `/supervisor/get_from_id` | `service` | `webots_ros::supervisor_get_from_id` | `int32 id`<br/>`---`<br/>`uint64 node` |
+| `/supervisor/get_from_device` | `service` | `webots_ros::supervisor_get_from_string` | `string value`<br/>`---`<br/>`uint64 node` |
 | `/supervisor/get_selected` | `service` | [`webots_ros::get_uint64`](ros-api.md#common-services) | |
 
 %tab-end
@@ -139,6 +146,10 @@ The function returns NULL if the given identifier doesn't match with any node of
 It is recommended to use this function only when knowing formerly the identifier (rather than looping on this function to retrieve all the nodes of a world).
 For example, when exporting an X3D file, its XML nodes are containing an `id` attribute which matches with the unique identifier described here.
 
+The `wb_supervisor_node_get_from_device` function retrieves the node's handle for a [Device](device.md) object.
+The function returns NULL if the given device is invalid or is an internal node of a PROTO.
+Note that in the ROS API the device name has to be used to retrieve the handle to the node.
+
 The `wb_supervisor_node_get_root` function returns a handle to the root node which is actually a [Group](group.md) node containing all the nodes visible at the top level in the scene tree window of Webots.
 Like any [Group](group.md) node, the root node has a MFNode field called "children" which can be parsed to read each node in the scene tree.
 An example of such a usage is provided in the "supervisor.wbt" sample worlds (located in the "projects/samples/devices/worlds" directory of Webots.

include/controller/c/webots/supervisor.h

@@ -88,6 +88,7 @@ WbNodeRef wb_supervisor_node_get_root();
 WbNodeRef wb_supervisor_node_get_self();
 int wb_supervisor_node_get_id(WbNodeRef node);
 WbNodeRef wb_supervisor_node_get_from_id(int id);
+WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag);
 WbNodeRef wb_supervisor_node_get_from_def(const char *def);
 WbNodeRef wb_supervisor_node_get_from_proto_def(WbNodeRef node, const char *def);
 WbNodeRef wb_supervisor_node_get_parent_node(WbNodeRef node);

include/controller/cpp/webots/Supervisor.hpp

@@ -15,6 +15,7 @@
 #ifndef SUPERVISOR_HPP
 #define SUPERVISOR_HPP
 
+#include <webots/Device.hpp>
 #include <webots/Node.hpp>
 #include <webots/Robot.hpp>
 
@@ -59,6 +60,8 @@ namespace webots {
     Node *getSelf() const;
     Node *getFromDef(const std::string &name) const;
     Node *getFromId(int id) const;
+    Node *getFromDevice(const Device *device) const;
+    Node *getFromDeviceTag(int tag) const;
     Node *getSelected() const;
 
     bool virtualRealityHeadsetIsUsed() const;

lib/controller/matlab/mgenerate.py

@@ -427,6 +427,7 @@ def gen_consts_from_list(list):
 gen(FUNC, "wb_supervisor_node_get_from_def(defname)", "supervisor")
 gen(FUNC, "wb_supervisor_node_get_from_proto_def(noderef, defname)", "supervisor")
 gen(FUNC, "wb_supervisor_node_get_from_id(id)", "supervisor")
+gen(FUNC, "wb_supervisor_node_get_from_device(tag)", "supervisor")
 gen(FUNC, "wb_supervisor_node_get_def(noderef)", "supervisor")
 gen(FUNC, "wb_supervisor_node_get_id(noderef)", "supervisor")
 gen(FUNC, "wb_supervisor_node_get_type(noderef)", "supervisor")

lib/controller/matlab/wb_supervisor_node_get_from_device.m

@@ -0,0 +1,6 @@
+function result = wb_supervisor_node_get_from_device(tag)
+% Usage: wb_supervisor_node_get_from_device(tag)
+% 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_get_from_device', tag);

projects/default/controllers/ros/RosSupervisor.cpp

@@ -65,6 +65,8 @@ RosSupervisor::RosSupervisor(Ros *ros, Supervisor *supervisor) {
     mRos->nodeHandle()->advertiseService((ros->name()) + "/supervisor/get_from_def", &RosSupervisor::getFromDefCallback, this);
   mGetFromIdServer =
     mRos->nodeHandle()->advertiseService((ros->name()) + "/supervisor/get_from_id", &RosSupervisor::getFromIdCallback, this);
+  mGetFromDeviceServer = mRos->nodeHandle()->advertiseService((ros->name()) + "/supervisor/get_from_device",
+                                                              &RosSupervisor::getFromDeviceCallback, this);
   mGetSelectedServer =
     mRos->nodeHandle()->advertiseService((ros->name()) + "/supervisor/get_selected", &RosSupervisor::getSelectedCallback, this);
   mVirtualRealityHeadsetGetOrientationServer =
@@ -212,6 +214,7 @@ RosSupervisor::~RosSupervisor() {
   mGetSelfServer.shutdown();
   mGetFromDefServer.shutdown();
   mGetFromIdServer.shutdown();
+  mGetFromDeviceServer.shutdown();
   mGetSelectedServer.shutdown();
   mVirtualRealityHeadsetGetOrientationServer.shutdown();
   mVirtualRealityHeadsetGetPositionServer.shutdown();
@@ -422,6 +425,15 @@ bool RosSupervisor::getFromIdCallback(webots_ros::supervisor_get_from_id::Reques
   return true;
 }
 
+// cppcheck-suppress constParameter
+bool RosSupervisor::getFromDeviceCallback(webots_ros::supervisor_get_from_string::Request &req,
+                                          webots_ros::supervisor_get_from_string::Response &res) {
+  assert(mSupervisor);
+  const Device *device = mRos->getDevice(req.value);
+  res.node = reinterpret_cast<uint64_t>(mSupervisor->getFromDevice(device));
+  return true;
+}
+
 bool RosSupervisor::getSelectedCallback(webots_ros::get_uint64::Request &req, webots_ros::get_uint64::Response &res) {
   assert(mSupervisor);
   res.value = reinterpret_cast<uint64_t>(mSupervisor->getSelected());

projects/default/controllers/ros/RosSupervisor.hpp

@@ -28,6 +28,7 @@
 #include <webots_ros/save_image.h>
 #include <webots_ros/supervisor_get_from_def.h>
 #include <webots_ros/supervisor_get_from_id.h>
+#include <webots_ros/supervisor_get_from_string.h>
 #include <webots_ros/supervisor_movie_start_recording.h>
 #include <webots_ros/supervisor_set_label.h>
 #include <webots_ros/supervisor_virtual_reality_headset_get_orientation.h>
@@ -117,6 +118,8 @@ class RosSupervisor {
   bool getFromDefCallback(webots_ros::supervisor_get_from_def::Request &req,
                           webots_ros::supervisor_get_from_def::Response &res);
   bool getFromIdCallback(webots_ros::supervisor_get_from_id::Request &req, webots_ros::supervisor_get_from_id::Response &res);
+  bool getFromDeviceCallback(webots_ros::supervisor_get_from_string::Request &req,
+                             webots_ros::supervisor_get_from_string::Response &res);
   bool getSelectedCallback(webots_ros::get_uint64::Request &req, webots_ros::get_uint64::Response &res);
 
   bool nodeGetIdCallback(webots_ros::node_get_id::Request &req, webots_ros::node_get_id::Response &res);
@@ -213,6 +216,7 @@ class RosSupervisor {
   ros::ServiceServer mGetSelfServer;
   ros::ServiceServer mGetFromDefServer;
   ros::ServiceServer mGetFromIdServer;
+  ros::ServiceServer mGetFromDeviceServer;
   ros::ServiceServer mGetSelectedServer;
   ros::ServiceServer mVirtualRealityHeadsetGetOrientationServer;
   ros::ServiceServer mVirtualRealityHeadsetGetPositionServer;

projects/default/controllers/ros/include/srv/supervisor_get_from_string.srv

@@ -0,0 +1,3 @@
+string value
+---
+uint64 node

projects/languages/ros/webots_ros/CMakeLists.txt

@@ -131,6 +131,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs sensor_msgs messag
     speaker_play_sound.srv
     supervisor_get_from_def.srv
     supervisor_get_from_id.srv
+    supervisor_get_from_string.srv
     supervisor_movie_start_recording.srv
     supervisor_set_label.srv
     supervisor_virtual_reality_headset_get_orientation.srv

projects/languages/ros/webots_ros/src/complete_test.cpp

@@ -122,6 +122,7 @@
 #include <webots_ros/speaker_speak.h>
 #include <webots_ros/supervisor_get_from_def.h>
 #include <webots_ros/supervisor_get_from_id.h>
+#include <webots_ros/supervisor_get_from_string.h>
 #include <webots_ros/supervisor_movie_start_recording.h>
 #include <webots_ros/supervisor_set_label.h>
 #include <webots_ros/supervisor_virtual_reality_headset_get_orientation.h>
@@ -3004,7 +3005,7 @@ int main(int argc, char **argv) {
   supervisor_node_get_type_name_client.call(supervisor_node_get_type_name_srv);
   ROS_INFO("Node got from field_get_node is of type %s.", supervisor_node_get_type_name_srv.response.name.c_str());
 
-  // supervisor_node_get_from_id
+  // supervisor_node_get_from_def
   supervisor_get_from_def_srv.request.name = "CONE";
   supervisor_get_from_def_srv.request.proto = 0;
   supervisor_get_from_def_client.call(supervisor_get_from_def_srv);
@@ -3048,6 +3049,22 @@ int main(int argc, char **argv) {
   node_get_id_client.shutdown();
   time_step_client.call(time_step_srv);
 
+  // supervisor_get_from_device
+  ros::ServiceClient supervisor_get_from_device_client;
+  webots_ros::supervisor_get_from_string supervisor_get_from_device_srv;
+  supervisor_get_from_device_client =
+    n.serviceClient<webots_ros::supervisor_get_from_string>(model_name + "/supervisor/get_from_device");
+  supervisor_get_from_device_srv.request.value = "compass";
+  supervisor_get_from_device_client.call(supervisor_get_from_device_srv);
+  uint64_t compass_node_from_device = supervisor_get_from_device_srv.response.node;
+  if (compass_node_from_device == from_def_node)
+    ROS_INFO("Compass node got successfully from tag.");
+  else
+    ROS_ERROR("Failed to call service supervisor_get_from_device.");
+
+  supervisor_get_from_device_client.shutdown();
+  time_step_client.call(time_step_srv);
+
   // node_set_velocity
   ros::ServiceClient node_velocity_client;
   webots_ros::node_set_velocity node_set_velocity_srv;

resources/languages/cpp/Supervisor.cpp

@@ -159,6 +159,15 @@ Node *Supervisor::getFromId(int id) const {
   return Node::findNode(nodeRef);
 }
 
+Node *Supervisor::getFromDevice(const Device *device) const {
+  return getFromDeviceTag(device->getTag());
+}
+
+Node *Supervisor::getFromDeviceTag(int tag) const {
+  WbNodeRef nodeRef = wb_supervisor_node_get_from_device(tag);
+  return Node::findNode(nodeRef);
+}
+
 Node *Supervisor::getSelected() const {
   WbNodeRef nodeRef = wb_supervisor_node_get_selected();
   return Node::findNode(nodeRef);

resources/languages/java/controller.i

@@ -1169,6 +1169,15 @@ namespace webots {
     return Node.findNode(cPtr);
   }
 
+  public Node getFromDevice(Device device) {
+    return getFromDeviceTag(device.getTag());
+  }
+
+  private Node getFromDeviceTag(int tag) {
+    long cPtr = wrapperJNI.Supervisor_getFromDeviceTagPrivate(swigCPtr, this, tag);
+    return Node.findNode(cPtr);
+  }
+
   public Node getSelected() {
     long cPtr = wrapperJNI.Supervisor_getSelectedPrivate(swigCPtr, this);
     return Node.findNode(cPtr);
@@ -1179,12 +1188,16 @@ namespace webots {
 %rename("getSelfPrivate") getSelf() const;
 %rename("getFromDefPrivate") getFromDef(const std::string &name) const;
 %rename("getFromIdPrivate") getFromId(int id) const;
+%rename("getFromDevicePrivate") getFromDevice(const Device *device) const;
+%rename("getFromDeviceTagPrivate") getFromDeviceTag(int tag) const;
 %rename("getSelectedPrivate") getSelected() const;
 
 %javamethodmodifiers getRoot() const "private"
 %javamethodmodifiers getSelf() const "private"
 %javamethodmodifiers getFromDef(const std::string &name) const "private"
 %javamethodmodifiers getFromId(int id) const "private"
+%javamethodmodifiers getFromDevice(const Device *device) const "private"
+%javamethodmodifiers getFromDeviceTag(int tag) const "private"
 %javamethodmodifiers getSelected() const "private"
 
 %include <webots/Supervisor.hpp>

resources/languages/python/controller.i

@@ -1110,4 +1110,6 @@ class AnsiCodes(object):
 //  Supervisor
 //----------------------------------------------------------------------------------------------
 
+%rename ("__internalGetFromDeviceTag") getFromDeviceTag;
+
 %include <webots/Supervisor.hpp>

src/lib/Controller/Controller.def

@@ -370,6 +370,7 @@ wb_supervisor_node_get_field
 wb_supervisor_node_get_from_def
 wb_supervisor_node_get_from_id
 wb_supervisor_node_get_from_proto_def
+wb_supervisor_node_get_from_device
 wb_supervisor_node_get_center_of_mass
 wb_supervisor_node_get_contact_point
 wb_supervisor_node_get_id

src/lib/Controller/api/messages.h

@@ -98,20 +98,21 @@
 #define C_SUPERVISOR_SAVE_WORLD 72
 #define C_SUPERVISOR_NODE_GET_FROM_ID 73
 #define C_SUPERVISOR_NODE_GET_FROM_DEF 74
-#define C_SUPERVISOR_NODE_GET_SELECTED 75
-#define C_SUPERVISOR_FIELD_GET_FROM_NAME 76
-#define C_SUPERVISOR_FIELD_GET_VALUE 77
-#define C_SUPERVISOR_FIELD_INSERT_VALUE 78
-#define C_SUPERVISOR_NODE_GET_POSITION 79
-#define C_SUPERVISOR_NODE_GET_ORIENTATION 80
-#define C_SUPERVISOR_NODE_GET_CENTER_OF_MASS 81
-#define C_SUPERVISOR_NODE_GET_CONTACT_POINTS 82
-#define C_SUPERVISOR_NODE_GET_STATIC_BALANCE 83
-#define C_SUPERVISOR_NODE_GET_VELOCITY 84
-#define C_SUPERVISOR_NODE_REMOVE_NODE 85
-#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_IS_USED 86
-#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_POSITION 87
-#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_ORIENTATION 88
+#define C_SUPERVISOR_NODE_GET_FROM_TAG 75
+#define C_SUPERVISOR_NODE_GET_SELECTED 76
+#define C_SUPERVISOR_FIELD_GET_FROM_NAME 77
+#define C_SUPERVISOR_FIELD_GET_VALUE 78
+#define C_SUPERVISOR_FIELD_INSERT_VALUE 79
+#define C_SUPERVISOR_NODE_GET_POSITION 80
+#define C_SUPERVISOR_NODE_GET_ORIENTATION 81
+#define C_SUPERVISOR_NODE_GET_CENTER_OF_MASS 82
+#define C_SUPERVISOR_NODE_GET_CONTACT_POINTS 83
+#define C_SUPERVISOR_NODE_GET_STATIC_BALANCE 84
+#define C_SUPERVISOR_NODE_GET_VELOCITY 85
+#define C_SUPERVISOR_NODE_REMOVE_NODE 86
+#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_IS_USED 87
+#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_POSITION 88
+#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_ORIENTATION 89
 
 // for the camera device
 // ctr -> sim