From 55bb7aa9ee527bc92e43c3da753411ccc3a8bc14 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 16:50:55 +0200 Subject: [PATCH 01/13] Add wb_supervisor_get_from_device function --- include/controller/c/webots/supervisor.h | 1 + include/controller/cpp/webots/Supervisor.hpp | 3 + lib/controller/matlab/mgenerate.py | 1 + .../wb_supervisor_node_get_from_device.m | 6 ++ .../default/controllers/ros/RosSupervisor.cpp | 11 +++ .../default/controllers/ros/RosSupervisor.hpp | 4 + .../languages/ros/webots_ros/CMakeLists.txt | 1 + .../ros/webots_ros/src/complete_test.cpp | 19 +++- resources/languages/cpp/Supervisor.cpp | 9 ++ resources/languages/java/controller.i | 13 +++ resources/languages/python/controller.i | 2 - src/lib/Controller/Controller.def | 1 + src/lib/Controller/api/messages.h | 29 ++++--- src/lib/Controller/api/supervisor.c | 87 +++++++++++++------ src/webots/core/WbLanguage.cpp | 7 +- .../nodes/utils/WbSupervisorUtilities.cpp | 63 +++++++++++--- .../nodes/utils/WbSupervisorUtilities.hpp | 4 +- 17 files changed, 199 insertions(+), 62 deletions(-) create mode 100644 lib/controller/matlab/wb_supervisor_node_get_from_device.m 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..795fd540284 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,14 @@ bool RosSupervisor::getFromIdCallback(webots_ros::supervisor_get_from_id::Reques return true; } +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..9619933cd21 100644 --- a/projects/default/controllers/ros/RosSupervisor.hpp +++ b/projects/default/controllers/ros/RosSupervisor.hpp @@ -29,6 +29,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/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..d57acc31594 100644 --- a/resources/languages/python/controller.i +++ b/resources/languages/python/controller.i @@ -1109,5 +1109,3 @@ class AnsiCodes(object): //---------------------------------------------------------------------------------------------- // Supervisor //---------------------------------------------------------------------------------------------- - -%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..f5f5fe45c26 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 < 0 || 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..381a9dd50e5 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 " @@ -695,11 +696,7 @@ void WbLanguage::cleanup() { WbLanguage::WbLanguage(int code, const QString &name, const QString &defaultFileSuffix, const QString &commentPrefix, bool isCompilable) : - mCode(code), - mName(name), - mDefaultFileSuffix(defaultFileSuffix), - mCommentPrefix(commentPrefix), - mIsCompilable(isCompilable) { + mCode(code), mName(name), mDefaultFileSuffix(defaultFileSuffix), mCommentPrefix(commentPrefix), mIsCompilable(isCompilable) { } WbLanguage::~WbLanguage() { diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.cpp b/src/webots/nodes/utils/WbSupervisorUtilities.cpp index 9f73b34adc7..c940dabfcc0 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,49 @@ 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); + if (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 +1337,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 +1361,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; From 4f1ccad16644f7bd206e28c29c1c140fbd7140ae Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 16:51:13 +0200 Subject: [PATCH 02/13] Add test --- .../supervisor_node/supervisor_node.c | 25 +++++++++++++++++++ tests/api/worlds/supervisor_node.wbt | 9 ++++++- 2 files changed, 33 insertions(+), 1 deletion(-) diff --git a/tests/api/controllers/supervisor_node/supervisor_node.c b/tests/api/controllers/supervisor_node/supervisor_node.c index e661c4b1a7a..65eef5e0d4d 100644 --- a/tests/api/controllers/supervisor_node/supervisor_node.c +++ b/tests/api/controllers/supervisor_node/supervisor_node.c @@ -30,6 +30,9 @@ int main(int argc, char **argv) { const char *charArray; 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); @@ -272,6 +275,28 @@ int main(int argc, char **argv) { time = wb_robot_get_time(); 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..3ac8aa45d01 100644 --- a/tests/api/worlds/supervisor_node.wbt +++ b/tests/api/worlds/supervisor_node.wbt @@ -4,7 +4,7 @@ WorldInfo { } Viewpoint { orientation 0.3434989325345835 -0.9106458211432591 -0.22963595489801025 5.24359 - position 0.676068 0.515729 0.632315 + position 1.4033374068186173 0.9049280452996868 1.1513673230933967 } Background { skyColor [ @@ -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" From a2e9a86f2779e4bd82f757dc321ddbea10dbd261 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 16:51:37 +0200 Subject: [PATCH 03/13] Add wb_supervisor_get_from_device documentation --- docs/reference/device.md | 2 +- docs/reference/supervisor.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) 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/supervisor.md b/docs/reference/supervisor.md index 5d85c79e948..32aae572821 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(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. From f902579f8bde79f6ae1987194451a2443d39f17e Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 16:52:55 +0200 Subject: [PATCH 04/13] Add new ros srv --- .../controllers/ros/include/srv/supervisor_get_from_string.srv | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 projects/default/controllers/ros/include/srv/supervisor_get_from_string.srv 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 From ccba68f07edd0cb403536075f7a5f94c351e360b Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:11:23 +0200 Subject: [PATCH 05/13] Fix wrong change and make getFromDeviceTag private on python too --- resources/languages/python/controller.i | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/resources/languages/python/controller.i b/resources/languages/python/controller.i index d57acc31594..d4d5a63cfac 100644 --- a/resources/languages/python/controller.i +++ b/resources/languages/python/controller.i @@ -1109,3 +1109,7 @@ class AnsiCodes(object): //---------------------------------------------------------------------------------------------- // Supervisor //---------------------------------------------------------------------------------------------- + +%rename ("__internalGetFromDeviceTag") getFromDeviceTag; + +%include From d83cca5665726973d7129639d67268c7690f1cb6 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:22:53 +0200 Subject: [PATCH 06/13] Add entry in changelog --- docs/reference/changelog-r2021.md | 7 +++++++ docs/reference/changelog.md | 1 + 2 files changed, 8 insertions(+) create mode 100644 docs/reference/changelog-r2021.md diff --git a/docs/reference/changelog-r2021.md b/docs/reference/changelog-r2021.md new file mode 100644 index 00000000000..6244d139c3a --- /dev/null +++ b/docs/reference/changelog-r2021.md @@ -0,0 +1,7 @@ +# Webots R2021 Change Log + +## Webots R2021a +Released on XXX YYth, 2021. + + - 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) From 8b67579ec664e8a88f632b1fe476b7bf7fc14dab Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:34:12 +0200 Subject: [PATCH 07/13] Fix clangformat and cppcheck errors --- .../default/controllers/ros/RosSupervisor.cpp | 1 + .../default/controllers/ros/RosSupervisor.hpp | 2 +- src/lib/Controller/api/supervisor.c | 2 +- .../nodes/utils/WbSupervisorUtilities.cpp | 37 +++++++++---------- 4 files changed, 21 insertions(+), 21 deletions(-) diff --git a/projects/default/controllers/ros/RosSupervisor.cpp b/projects/default/controllers/ros/RosSupervisor.cpp index 795fd540284..82507db7695 100644 --- a/projects/default/controllers/ros/RosSupervisor.cpp +++ b/projects/default/controllers/ros/RosSupervisor.cpp @@ -425,6 +425,7 @@ 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); diff --git a/projects/default/controllers/ros/RosSupervisor.hpp b/projects/default/controllers/ros/RosSupervisor.hpp index 9619933cd21..e59ad772f97 100644 --- a/projects/default/controllers/ros/RosSupervisor.hpp +++ b/projects/default/controllers/ros/RosSupervisor.hpp @@ -28,8 +28,8 @@ #include #include #include -#include #include +#include #include #include #include diff --git a/src/lib/Controller/api/supervisor.c b/src/lib/Controller/api/supervisor.c index f5f5fe45c26..ff5cff0a914 100644 --- a/src/lib/Controller/api/supervisor.c +++ b/src/lib/Controller/api/supervisor.c @@ -1591,7 +1591,7 @@ WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag) { if (!robot_check_supervisor(__FUNCTION__)) return NULL; - if (tag < 0 || tag >= robot_get_number_of_devices()) { + if (tag >= robot_get_number_of_devices()) { fprintf(stderr, "Error: %s() called with an invalid 'tag' argument.\n", __FUNCTION__); return NULL; } diff --git a/src/webots/nodes/utils/WbSupervisorUtilities.cpp b/src/webots/nodes/utils/WbSupervisorUtilities.cpp index c940dabfcc0..d3a61bdf1f8 100644 --- a/src/webots/nodes/utils/WbSupervisorUtilities.cpp +++ b/src/webots/nodes/utils/WbSupervisorUtilities.cpp @@ -649,26 +649,25 @@ void WbSupervisorUtilities::handleMessage(QDataStream &stream) { if (!device) return; const WbBaseNode *baseNode = dynamic_cast(device); - if (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); + 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: { From 4b2b85efb3519b6110e13acbdd319b9ff563acd3 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:38:26 +0200 Subject: [PATCH 08/13] Add new changelog page in menu --- docs/reference/menu.md | 1 + 1 file changed, 1 insertion(+) 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) From 746ddf3c3fb3c32dd0e857eef3cdbaf9c72f9393 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:48:33 +0200 Subject: [PATCH 09/13] Fix clang format --- src/webots/core/WbLanguage.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/webots/core/WbLanguage.cpp b/src/webots/core/WbLanguage.cpp index 381a9dd50e5..c80351658e9 100644 --- a/src/webots/core/WbLanguage.cpp +++ b/src/webots/core/WbLanguage.cpp @@ -696,7 +696,11 @@ void WbLanguage::cleanup() { WbLanguage::WbLanguage(int code, const QString &name, const QString &defaultFileSuffix, const QString &commentPrefix, bool isCompilable) : - mCode(code), mName(name), mDefaultFileSuffix(defaultFileSuffix), mCommentPrefix(commentPrefix), mIsCompilable(isCompilable) { + mCode(code), + mName(name), + mDefaultFileSuffix(defaultFileSuffix), + mCommentPrefix(commentPrefix), + mIsCompilable(isCompilable) { } WbLanguage::~WbLanguage() { From 65ed8bf0187cbd9c9056822085e6d7900f0b07f1 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Tue, 4 Aug 2020 17:56:34 +0200 Subject: [PATCH 10/13] Fix clang format --- .../controllers/supervisor_node/supervisor_node.c | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/tests/api/controllers/supervisor_node/supervisor_node.c b/tests/api/controllers/supervisor_node/supervisor_node.c index 65eef5e0d4d..16b92a70912 100644 --- a/tests/api/controllers/supervisor_node/supervisor_node.c +++ b/tests/api/controllers/supervisor_node/supervisor_node.c @@ -30,7 +30,7 @@ int main(int argc, char **argv) { const char *charArray; const double *doubleArray; int i; - + WbDeviceTag sick_lidar = wb_robot_get_device("Sick LMS 291"); WbDeviceTag compass = wb_robot_get_device("compass"); @@ -275,23 +275,24 @@ int main(int argc, char **argv) { time = wb_robot_get_time(); 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"); + 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"); From 288f4744728f36d4f91a500dc2d3f50fcbccf67a Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Thu, 6 Aug 2020 08:14:42 +0200 Subject: [PATCH 11/13] reset test viewpoint position --- tests/api/worlds/supervisor_node.wbt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/api/worlds/supervisor_node.wbt b/tests/api/worlds/supervisor_node.wbt index 3ac8aa45d01..d9c65195ff6 100644 --- a/tests/api/worlds/supervisor_node.wbt +++ b/tests/api/worlds/supervisor_node.wbt @@ -4,7 +4,7 @@ WorldInfo { } Viewpoint { orientation 0.3434989325345835 -0.9106458211432591 -0.22963595489801025 5.24359 - position 1.4033374068186173 0.9049280452996868 1.1513673230933967 + position 0.676068 0.515729 0.632315 } Background { skyColor [ From e5535d46792178f31f02980221b67f8edd9b2355 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Thu, 6 Aug 2020 10:36:52 +0200 Subject: [PATCH 12/13] Update docs/reference/changelog-r2021.md Co-authored-by: Olivier Michel --- docs/reference/changelog-r2021.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/reference/changelog-r2021.md b/docs/reference/changelog-r2021.md index 6244d139c3a..3b7421bbda2 100644 --- a/docs/reference/changelog-r2021.md +++ b/docs/reference/changelog-r2021.md @@ -1,7 +1,7 @@ # Webots R2021 Change Log ## Webots R2021a -Released on XXX YYth, 2021. +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)). From 3ac96b85cbd98596d458801dc1a3e17f42623ea1 Mon Sep 17 00:00:00 2001 From: Stefania Pedrazzi Date: Thu, 6 Aug 2020 10:37:02 +0200 Subject: [PATCH 13/13] Update docs/reference/supervisor.md Co-authored-by: Olivier Michel --- docs/reference/supervisor.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/reference/supervisor.md b/docs/reference/supervisor.md index 32aae572821..78d1ded3185 100644 --- a/docs/reference/supervisor.md +++ b/docs/reference/supervisor.md @@ -98,7 +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(tag) +node = wb_supervisor_node_get_from_device(tag) node = wb_supervisor_node_get_selected() ```