diff --git a/docs/reference/changelog-r2021.md b/docs/reference/changelog-r2021.md new file mode 100644 index 00000000000..3b7421bbda2 --- /dev/null +++ b/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)). diff --git a/docs/reference/changelog.md b/docs/reference/changelog.md index 1e5837bcfca..f2ae45551a6 100644 --- a/docs/reference/changelog.md +++ b/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) diff --git a/docs/reference/device.md b/docs/reference/device.md index 79872441530..189bfbe2dee 100644 --- a/docs/reference/device.md +++ b/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 diff --git a/docs/reference/menu.md b/docs/reference/menu.md index 6a099ccda6e..19ef3953d16 100644 --- a/docs/reference/menu.md +++ b/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) diff --git a/docs/reference/supervisor.md b/docs/reference/supervisor.md index 5d85c79e948..78d1ded3185 100644 --- a/docs/reference/supervisor.md +++ b/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`
`uint64 proto`
`---`
`uint64 node` | | `/supervisor/get_from_id` | `service` | `webots_ros::supervisor_get_from_id` | `int32 id`
`---`
`uint64 node` | +| `/supervisor/get_from_device` | `service` | `webots_ros::supervisor_get_from_string` | `string value`
`---`
`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. diff --git a/include/controller/c/webots/supervisor.h b/include/controller/c/webots/supervisor.h index b9db11caa7b..66ddd86dd0c 100644 --- a/include/controller/c/webots/supervisor.h +++ b/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); diff --git a/include/controller/cpp/webots/Supervisor.hpp b/include/controller/cpp/webots/Supervisor.hpp index 2740f2d53d3..b180e9a7aa3 100644 --- a/include/controller/cpp/webots/Supervisor.hpp +++ b/include/controller/cpp/webots/Supervisor.hpp @@ -15,6 +15,7 @@ #ifndef SUPERVISOR_HPP #define SUPERVISOR_HPP +#include #include #include @@ -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; diff --git a/lib/controller/matlab/mgenerate.py b/lib/controller/matlab/mgenerate.py index 05b5a9be3b2..0e68aab4722 100755 --- a/lib/controller/matlab/mgenerate.py +++ b/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") diff --git a/lib/controller/matlab/wb_supervisor_node_get_from_device.m b/lib/controller/matlab/wb_supervisor_node_get_from_device.m new file mode 100644 index 00000000000..35b49010e39 --- /dev/null +++ b/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 here + +result = calllib('libController', 'wb_supervisor_node_get_from_device', tag); diff --git a/projects/default/controllers/ros/RosSupervisor.cpp b/projects/default/controllers/ros/RosSupervisor.cpp index 8693572717e..82507db7695 100644 --- a/projects/default/controllers/ros/RosSupervisor.cpp +++ b/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(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(mSupervisor->getSelected()); diff --git a/projects/default/controllers/ros/RosSupervisor.hpp b/projects/default/controllers/ros/RosSupervisor.hpp index 5190e37fbf7..e59ad772f97 100644 --- a/projects/default/controllers/ros/RosSupervisor.hpp +++ b/projects/default/controllers/ros/RosSupervisor.hpp @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -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; diff --git a/projects/default/controllers/ros/include/srv/supervisor_get_from_string.srv b/projects/default/controllers/ros/include/srv/supervisor_get_from_string.srv new file mode 100644 index 00000000000..758b7792e68 --- /dev/null +++ b/projects/default/controllers/ros/include/srv/supervisor_get_from_string.srv @@ -0,0 +1,3 @@ +string value +--- +uint64 node diff --git a/projects/languages/ros/webots_ros/CMakeLists.txt b/projects/languages/ros/webots_ros/CMakeLists.txt index dff5862f926..58c96936b8c 100644 --- a/projects/languages/ros/webots_ros/CMakeLists.txt +++ b/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 diff --git a/projects/languages/ros/webots_ros/src/complete_test.cpp b/projects/languages/ros/webots_ros/src/complete_test.cpp index 95f2f480eaf..7d54d6e5822 100644 --- a/projects/languages/ros/webots_ros/src/complete_test.cpp +++ b/projects/languages/ros/webots_ros/src/complete_test.cpp @@ -122,6 +122,7 @@ #include #include #include +#include #include #include #include @@ -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(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; diff --git a/resources/languages/cpp/Supervisor.cpp b/resources/languages/cpp/Supervisor.cpp index 71e11ba0771..9e018a7ddbc 100644 --- a/resources/languages/cpp/Supervisor.cpp +++ b/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); diff --git a/resources/languages/java/controller.i b/resources/languages/java/controller.i index 85c10636f5f..b0a06f24063 100644 --- a/resources/languages/java/controller.i +++ b/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 diff --git a/resources/languages/python/controller.i b/resources/languages/python/controller.i index 2358bbc3514..d4d5a63cfac 100644 --- a/resources/languages/python/controller.i +++ b/resources/languages/python/controller.i @@ -1110,4 +1110,6 @@ class AnsiCodes(object): // Supervisor //---------------------------------------------------------------------------------------------- +%rename ("__internalGetFromDeviceTag") getFromDeviceTag; + %include diff --git a/src/lib/Controller/Controller.def b/src/lib/Controller/Controller.def index d6e2def8988..b0b86b65492 100644 --- a/src/lib/Controller/Controller.def +++ b/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 diff --git a/src/lib/Controller/api/messages.h b/src/lib/Controller/api/messages.h index f4362a1a03a..b3ade22939a 100644 --- a/src/lib/Controller/api/messages.h +++ b/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 diff --git a/src/lib/Controller/api/supervisor.c b/src/lib/Controller/api/supervisor.c index a52304b17a9..ff5cff0a914 100644 --- a/src/lib/Controller/api/supervisor.c +++ b/src/lib/Controller/api/supervisor.c @@ -95,6 +95,7 @@ typedef struct WbNodeStructPrivate { bool is_proto; bool is_proto_internal; WbNodeRef parent_proto; + int tag; WbNodeRef next; } WbNodeStruct; @@ -166,6 +167,16 @@ static WbNodeRef find_node_by_def(const char *def_name, WbNodeRef parent_proto) return NULL; } +static WbNodeRef find_node_by_tag(int tag) { + WbNodeRef node = node_list; + while (node) { + if (node->tag == tag) + return node; + node = node->next; + } + return NULL; +} + static bool is_node_ref_valid(WbNodeRef n) { if (!n) return false; @@ -278,7 +289,7 @@ static void remove_internal_proto_nodes_and_fields_from_list() { } } -static void add_node_to_list(int uid, WbNodeType type, const char *model_name, const char *def_name, int parent_id, +static void add_node_to_list(int uid, WbNodeType type, const char *model_name, const char *def_name, int tag, int parent_id, bool is_proto) { WbNodeRef nodeInList = find_node_by_id(uid); if (nodeInList) { @@ -310,6 +321,7 @@ static void add_node_to_list(int uid, WbNodeType type, const char *model_name, c n->is_proto = is_proto; n->is_proto_internal = false; n->parent_proto = NULL; + n->tag = tag; n->next = node_list; node_list = n; } @@ -352,13 +364,13 @@ static bool save_status = true; static bool save_request = false; static char *save_filename = NULL; static int node_id = -1; +static int node_tag = -1; static WbNodeRef node_to_remove = NULL; static bool allow_search_in_proto = false; static const char *node_def_name = NULL; static int proto_id = -1; static const char *requested_field_name = NULL; static bool node_get_selected = false; -static int selected_node_id = -1; static int node_ref = 0; static WbNodeRef root_ref = NULL; static WbNodeRef self_node_ref = NULL; @@ -458,6 +470,9 @@ static void supervisor_write_request(WbDevice *d, WbRequest *r) { request_write_uchar(r, C_SUPERVISOR_NODE_GET_FROM_DEF); request_write_string(r, node_def_name); request_write_int32(r, proto_id); + } else if (node_tag > 0) { + request_write_uchar(r, C_SUPERVISOR_NODE_GET_FROM_TAG); + request_write_int32(r, node_tag); } else if (node_get_selected) { request_write_uchar(r, C_SUPERVISOR_NODE_GET_SELECTED); } else if (requested_field_name) { @@ -753,41 +768,37 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { const bool is_proto_internal = request_read_uchar(r) == 1; const char *model_name = request_read_string(r); const char *def_name = request_read_string(r); - add_node_to_list(self_uid, WB_NODE_ROBOT, model_name, def_name, 0, is_proto); // add self node + add_node_to_list(self_uid, WB_NODE_ROBOT, model_name, def_name, 0, is_proto, 0); // add self node self_node_ref = node_list; self_node_ref->is_proto_internal = is_proto_internal; } break; case C_SUPERVISOR_NODE_GET_FROM_DEF: { const int uid = request_read_uint32(r); const WbNodeType type = request_read_uint32(r); + const int tag = request_read_int32(r); const int parent_uid = request_read_uint32(r); const bool is_proto = request_read_uchar(r) == 1; const char *model_name = request_read_string(r); if (uid) { - add_node_to_list(uid, type, model_name, node_def_name, parent_uid, is_proto); + add_node_to_list(uid, type, model_name, node_def_name, tag, parent_uid, is_proto); node_id = uid; } } break; - case C_SUPERVISOR_NODE_GET_SELECTED: { - selected_node_id = request_read_uint32(r); - const WbNodeType type = request_read_uint32(r); - const int parent_uid = request_read_uint32(r); - const bool is_proto = request_read_uchar(r) == 1; - const char *model_name = request_read_string(r); - const char *def = request_read_string(r); - if (selected_node_id) - add_node_to_list(selected_node_id, type, model_name, def, parent_uid, is_proto); - } break; - case C_SUPERVISOR_NODE_GET_FROM_ID: { + case C_SUPERVISOR_NODE_GET_SELECTED: + case C_SUPERVISOR_NODE_GET_FROM_ID: + case C_SUPERVISOR_NODE_GET_FROM_TAG: { const int uid = request_read_uint32(r); const WbNodeType type = request_read_uint32(r); + const int tag = request_read_int32(r); const int parent_uid = request_read_uint32(r); const bool is_proto = request_read_uchar(r) == 1; const bool is_proto_internal = request_read_uchar(r) == 1; const char *model_name = request_read_string(r); const char *def_name = request_read_string(r); - if (uid && !is_proto_internal) - add_node_to_list(uid, type, model_name, def_name, parent_uid, is_proto); + if (uid && !is_proto_internal) { + add_node_to_list(uid, type, model_name, def_name, tag, parent_uid, is_proto); + node_id = uid; + } } break; case C_SUPERVISOR_FIELD_GET_FROM_NAME: { const int field_ref = request_read_int32(r); @@ -858,11 +869,12 @@ static void supervisor_read_answer(WbDevice *d, WbRequest *r) { f->data.sf_node_uid = request_read_uint32(r); // 0 => NULL node if (f->data.sf_node_uid) { const WbNodeType type = request_read_uint32(r); + const int tag = request_read_int32(r); const int parent_uid = request_read_uint32(r); const bool is_proto = request_read_uchar(r) == 1; const char *model_name = request_read_string(r); const char *def_name = request_read_string(r); - add_node_to_list(f->data.sf_node_uid, type, model_name, def_name, parent_uid, is_proto); + add_node_to_list(f->data.sf_node_uid, type, model_name, def_name, tag, parent_uid, is_proto); } break; default: @@ -1126,7 +1138,7 @@ void wb_supervisor_init(WbDevice *d) { d->write_request = supervisor_write_request; d->read_answer = supervisor_read_answer; d->cleanup = supervisor_cleanup; - add_node_to_list(0, WB_NODE_GROUP, wb_node_get_name(WB_NODE_GROUP), NULL, -1, false); // create root node + add_node_to_list(0, WB_NODE_GROUP, wb_node_get_name(WB_NODE_GROUP), NULL, 0, -1, false); // create root node root_ref = node_list; } @@ -1575,6 +1587,33 @@ WbNodeRef wb_supervisor_node_get_from_def(const char *def) { return result; } +WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag) { + if (!robot_check_supervisor(__FUNCTION__)) + return NULL; + + if (tag >= robot_get_number_of_devices()) { + fprintf(stderr, "Error: %s() called with an invalid 'tag' argument.\n", __FUNCTION__); + return NULL; + } + + robot_mutex_lock_step(); + + // search if node is already present in node_list + WbNodeRef result = find_node_by_tag(tag); + if (!result) { + // otherwise: need to talk to Webots + node_tag = tag; + node_id = -1; + wb_robot_flush_unlocked(); + if (node_id >= 0) + result = find_node_by_id(node_id); + node_tag = -1; + node_id = -1; + } + robot_mutex_unlock_step(); + return result; +} + bool wb_supervisor_node_is_proto(WbNodeRef node) { if (!robot_check_supervisor(__FUNCTION__)) return false; @@ -1654,14 +1693,12 @@ WbNodeRef wb_supervisor_node_get_selected() { robot_mutex_lock_step(); WbNodeRef result = NULL; - WbNodeRef node_list_before = node_list; node_get_selected = true; - selected_node_id = -1; + node_id = -1; wb_robot_flush_unlocked(); - if (node_list != node_list_before) - result = node_list; - else if (selected_node_id >= 0) - result = find_node_by_id(selected_node_id); + if (node_id >= 0) + result = find_node_by_id(node_id); + node_id = -1; node_get_selected = false; robot_mutex_unlock_step(); diff --git a/src/webots/core/WbLanguage.cpp b/src/webots/core/WbLanguage.cpp index 7a028602f4a..c80351658e9 100644 --- a/src/webots/core/WbLanguage.cpp +++ b/src/webots/core/WbLanguage.cpp @@ -419,6 +419,7 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "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_id " "wb_supervisor_node_get_number_of_contact_points " "wb_supervisor_node_get_orientation " diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.cpp b/src/webots/nodes/utils/WbSupervisorUtilities.cpp index 9f73b34adc7..d3a61bdf1f8 100644 --- a/src/webots/nodes/utils/WbSupervisorUtilities.cpp +++ b/src/webots/nodes/utils/WbSupervisorUtilities.cpp @@ -16,6 +16,7 @@ #include "WbAbstractCamera.hpp" #include "WbApplication.hpp" +#include "WbDevice.hpp" #include "WbDictionary.hpp" #include "WbField.hpp" #include "WbFieldModel.hpp" @@ -274,8 +275,7 @@ void WbSupervisorUtilities::initControllerRequests() { mFoundFieldType = 0; mFoundFieldCount = -1; mFoundFieldIsInternal = false; - mGetSelectedNode = false; - mGetFromId = false; + mGetNodeRequest = 0; mNeedToResetSimulation = false; mNodeGetPosition = NULL; mNodeGetOrientation = NULL; @@ -595,10 +595,12 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { const WbBaseNode *node = dynamic_cast(WbNode::findNode(id)); if (node) { // since 8.6 -> each message has its own mechanism - mGetFromId = true; + mGetNodeRequest = C_SUPERVISOR_NODE_GET_FROM_ID; mCurrentDefName = node->defName(); mFoundNodeUniqueId = node->uniqueId(); mFoundNodeType = node->nodeType(); + const WbDevice *device = dynamic_cast(node); + mFoundNodeTag = (device && mRobot->findDevice(device->tag()) == device) ? device->tag() : -1; mFoundNodeModelName = node->modelName(); mFoundNodeParentUniqueId = (node->parentNode() ? node->parentNode()->uniqueId() : -1); mFoundNodeIsProto = node->isProtoInstance(); @@ -619,6 +621,8 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { baseNode = NULL; mFoundNodeUniqueId = baseNode ? baseNode->uniqueId() : 0; mFoundNodeType = baseNode ? baseNode->nodeType() : 0; + const WbDevice *device = dynamic_cast(baseNode); + mFoundNodeTag = (device && mRobot->findDevice(device->tag()) == device) ? device->tag() : -1; mFoundNodeModelName = baseNode ? baseNode->modelName() : QString(); mFoundNodeIsProtoInternal = false; if (baseNode) { @@ -636,15 +640,48 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { } return; } + case C_SUPERVISOR_NODE_GET_FROM_TAG: { + int tag; + stream >> tag; + + mFoundNodeUniqueId = -1; + const WbDevice *device = mRobot->findDevice(tag); + if (!device) + return; + const WbBaseNode *baseNode = dynamic_cast(device); + assert(baseNode); + mFoundNodeIsProtoInternal = + baseNode->parentNode() != WbWorld::instance()->root() && !WbNodeUtilities::isVisible(baseNode->parentField()); + if (mFoundNodeIsProtoInternal) + return; + mGetNodeRequest = C_SUPERVISOR_NODE_GET_FROM_TAG; + mCurrentDefName = baseNode->defName(); + mFoundNodeUniqueId = baseNode->uniqueId(); + mFoundNodeType = baseNode->nodeType(); + mFoundNodeTag = tag; + mFoundNodeModelName = baseNode->modelName(); + if (baseNode->parentNode()) { + if (baseNode->parentNode() != WbWorld::instance()->root()) + mFoundNodeParentUniqueId = baseNode->parentNode()->uniqueId(); + else + mFoundNodeParentUniqueId = 0; + } + mFoundNodeIsProto = baseNode->isProtoInstance(); + connect(baseNode, &WbNode::defUseNameChanged, this, &WbSupervisorUtilities::notifyNodeUpdate, Qt::UniqueConnection); + return; + } case C_SUPERVISOR_NODE_GET_SELECTED: { const WbBaseNode *baseNode = dynamic_cast(WbSelection::instance()->selectedNode()); if (baseNode) { - mGetSelectedNode = true; + mGetNodeRequest = C_SUPERVISOR_NODE_GET_SELECTED; mCurrentDefName = baseNode->defName(); mFoundNodeUniqueId = baseNode->uniqueId(); mFoundNodeType = baseNode->nodeType(); + const WbDevice *device = dynamic_cast(baseNode); + mFoundNodeTag = (device && mRobot->findDevice(device->tag()) == device) ? device->tag() : -1; mFoundNodeModelName = baseNode->modelName(); mFoundNodeParentUniqueId = -1; + mFoundNodeIsProtoInternal = false; if (baseNode->parentNode()) { if (baseNode->parentNode() != WbWorld::instance()->root()) mFoundNodeParentUniqueId = baseNode->parentNode()->uniqueId(); @@ -1299,6 +1336,8 @@ void WbSupervisorUtilities::writeNode(QDataStream &stream, const WbBaseNode *bas assert(baseNode); stream << (int)baseNode->uniqueId(); stream << (int)baseNode->nodeType(); + const WbDevice *device = dynamic_cast(baseNode); + stream << (int)((device && mRobot->findDevice(device->tag()) == device) ? device->tag() : -1); stream << (int)(baseNode->parentNode() ? baseNode->parentNode()->uniqueId() : -1); stream << (unsigned char)baseNode->isProtoInstance(); const QByteArray &modelName = baseNode->modelName().toUtf8(); @@ -1321,32 +1360,29 @@ void WbSupervisorUtilities::writeAnswer(QDataStream &stream) { } mUpdatedNodeIds.clear(); } - if (mGetFromId || mGetSelectedNode) { - mGetFromId = false; + if (mGetNodeRequest > 0) { stream << (short unsigned int)0; - if (mGetSelectedNode) { - mGetSelectedNode = false; - stream << (unsigned char)C_SUPERVISOR_NODE_GET_SELECTED; - } else - stream << (unsigned char)C_SUPERVISOR_NODE_GET_FROM_ID; + stream << (unsigned char)mGetNodeRequest; stream << (int)mFoundNodeUniqueId; stream << (int)mFoundNodeType; + stream << (int)mFoundNodeTag; stream << (int)mFoundNodeParentUniqueId; stream << (unsigned char)mFoundNodeIsProto; - if (mGetFromId) - stream << (unsigned char)mFoundNodeIsProtoInternal; + stream << (unsigned char)mFoundNodeIsProtoInternal; const QByteArray &modelName = mFoundNodeModelName.toUtf8(); const QByteArray &defName = mCurrentDefName.toUtf8(); stream.writeRawData(modelName.constData(), modelName.size() + 1); stream.writeRawData(defName.constData(), defName.size() + 1); mFoundNodeUniqueId = -1; mCurrentDefName.clear(); + mGetNodeRequest = 0; } if (mFoundNodeUniqueId != -1) { stream << (short unsigned int)0; stream << (unsigned char)C_SUPERVISOR_NODE_GET_FROM_DEF; stream << (int)mFoundNodeUniqueId; stream << (int)mFoundNodeType; + stream << (int)mFoundNodeTag; stream << (int)mFoundNodeParentUniqueId; stream << (unsigned char)mFoundNodeIsProto; QByteArray s = mFoundNodeModelName.toUtf8(); diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.hpp b/src/webots/nodes/utils/WbSupervisorUtilities.hpp index 0ffaf01cd47..d6c56a20eba 100644 --- a/src/webots/nodes/utils/WbSupervisorUtilities.hpp +++ b/src/webots/nodes/utils/WbSupervisorUtilities.hpp @@ -69,6 +69,7 @@ private slots: WbRobot *mRobot; int mFoundNodeUniqueId; int mFoundNodeType; + int mFoundNodeTag; QString mFoundNodeModelName; QString mCurrentDefName; int mFoundNodeParentUniqueId; @@ -78,8 +79,7 @@ private slots: int mFoundFieldType; int mFoundFieldCount; bool mFoundFieldIsInternal; - bool mGetSelectedNode; - bool mGetFromId; + int mGetNodeRequest; bool mNeedToResetSimulation; QList mUpdatedNodeIds; WbTransform *mNodeGetPosition; diff --git a/tests/api/controllers/supervisor_node/supervisor_node.c b/tests/api/controllers/supervisor_node/supervisor_node.c index e661c4b1a7a..16b92a70912 100644 --- a/tests/api/controllers/supervisor_node/supervisor_node.c +++ b/tests/api/controllers/supervisor_node/supervisor_node.c @@ -31,6 +31,9 @@ int main(int argc, char **argv) { const double *doubleArray; int i; + WbDeviceTag sick_lidar = wb_robot_get_device("Sick LMS 291"); + WbDeviceTag compass = wb_robot_get_device("compass"); + double time = wb_robot_get_time(); ts_assert_boolean_equal(time == 0.0, "Starting time is wrong. Expected=0.0. Received=%f\n", time); @@ -273,6 +276,29 @@ int main(int argc, char **argv) { ts_assert_boolean_equal(time == 0.001 * TIME_STEP, "Ending time is wrong. Expected=%f. Received=%f\n", 0.001 * TIME_STEP, time); + wb_robot_step(TIME_STEP); + + WbNodeRef self_by_tag = wb_supervisor_node_get_from_device(0); + ts_assert_pointer_not_null(self_by_tag, "Null node reference to self node by device tag"); + ts_assert_boolean_equal(self_by_tag == wb_supervisor_node_get_self(), "Invalid node reference to self node by tag"); + + WbNodeRef compass_node = wb_supervisor_node_get_from_device(compass); + ts_assert_pointer_not_null(compass_node, "Invalid compass node reference from device tag"); + ts_assert_string_equal(wb_supervisor_node_get_def(compass_node), "COMPASS", "Wrong compass node reference"); + + WbNodeRef sick_node = wb_supervisor_node_get_from_device(sick_lidar); + ts_assert_pointer_not_null(sick_node, "Invalid Sick node reference from device tag"); + WbFieldRef sick_translation_field = wb_supervisor_node_get_field(sick_node, "translation"); + ts_assert_pointer_not_null(sick_translation_field, "Invalid translation field for Sick node"); + const double *sick_translation = wb_supervisor_field_get_sf_vec3f(sick_translation_field); + ts_assert_vec3_equal(sick_translation[0], sick_translation[1], sick_translation[2], 0.39, 0.1, 0.4, + "Invalid translation value for Sick node"); + + WbDeviceTag kinect_color = wb_robot_get_device("kinect color"); + ts_assert_int_not_equal(kinect_color, 0, "Kinect color camera not found"); + WbNodeRef kinect_node = wb_supervisor_node_get_from_device(kinect_color); + ts_assert_pointer_null(kinect_node, "Kinect node reference from device tag returned even if hidden"); + ts_send_success(); return EXIT_SUCCESS; } diff --git a/tests/api/worlds/supervisor_node.wbt b/tests/api/worlds/supervisor_node.wbt index 8e97ca6978a..d9c65195ff6 100644 --- a/tests/api/worlds/supervisor_node.wbt +++ b/tests/api/worlds/supervisor_node.wbt @@ -65,8 +65,15 @@ DEF ROBOT Robot { } DEF Test Robot { children [ + DEF COMPASS Compass { + } TestSuiteEmitter { } + SickLms291 { + translation 0.39 0.1 0.4 + } + Kinect { + } ] name "supervisor" controller "supervisor_node"