From 09872c76c3aef31662b27d181b8b5448b2f5f1b8 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 5 Jan 2022 09:14:54 +0100 Subject: [PATCH 01/39] new step_begin and step_end exports --- include/controller/c/webots/robot.h | 2 ++ src/controller/c/robot.c | 45 +++++++++++++++++------------ 2 files changed, 29 insertions(+), 18 deletions(-) diff --git a/include/controller/c/webots/robot.h b/include/controller/c/webots/robot.h index ff2d974ef17..affd52b886c 100644 --- a/include/controller/c/webots/robot.h +++ b/include/controller/c/webots/robot.h @@ -69,6 +69,8 @@ int wb_robot_init_msvc(); // internally, this function just calls wb_robot_init #define wb_robot_init() (setvbuf(stdout, NULL, _IONBF, 0), setvbuf(stderr, NULL, _IONBF, 0), wb_robot_init_msvc()) #endif +int wb_robot_step_begin(int duration); +int wb_robot_step_end(); // milliseconds int wb_robot_step(int duration); // milliseconds #ifdef __CYGWIN__ // In that case, we need to flush explicitly the stdout/stdin streams otherwise they are buffered diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 9d8fa177b84..60bf92854e7 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -554,21 +554,6 @@ void robot_mutex_unlock_step() { wb_robot_mutex_unlock(robot_step_mutex); } -void robot_step_begin(int duration) { - motion_step_all(duration); - robot_send_request(duration); -} - -int robot_step_end() { - keyboard_step_end(); - joystick_step_end(); - robot_read_data(); - if (robot.webots_exit == WEBOTS_EXIT_FALSE) - return scheduler_actual_step; - - return -1; -} - WbDeviceTag robot_get_device_tag(const WbDevice *d) { WbDeviceTag tag; for (tag = 0; tag < robot.n_device; tag++) { @@ -810,7 +795,7 @@ void wbr_robot_battery_sensor_set_value(double value) { robot.battery_value = value; } -int wb_robot_step(int duration) { +int wb_robot_step_begin(int duration) { if (!robot.client_exit) html_robot_window_step(duration); @@ -846,8 +831,23 @@ int wb_robot_step(int duration) { robot_window_pre_update_gui(); robot_mutex_lock_step(); - robot_step_begin(duration); - int e = robot_step_end(); + motion_step_all(duration); + robot_send_request(duration); + + return 0; +} + +int wb_robot_step_end() { + if (robot.webots_exit == WEBOTS_EXIT_NOW) + return -1; + + keyboard_step_end(); + joystick_step_end(); + robot_read_data(); + + int e = -1; + if (robot.webots_exit == WEBOTS_EXIT_FALSE) + e = scheduler_actual_step; if (e != -1 && wb_robot_get_mode() == WB_MODE_REMOTE_CONTROL && remote_control_has_failed()) wb_robot_set_mode(0, NULL); @@ -858,6 +858,15 @@ int wb_robot_step(int duration) { return e; } +int wb_robot_step(int duration) { + int e = wb_robot_step_begin(duration); + if(e==-1) + return e; + e = wb_robot_step_end(); + + return e; +} + WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, int timeout) { // check that the devices associated with the event type are enabled bool valid = event_type == WB_EVENT_NO_EVENT; From 56a1fd55ee3dbba021a3bb21d2bf5ddcca4a8e57 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 5 Jan 2022 14:32:58 +0100 Subject: [PATCH 02/39] unlock the mutex after step_begin --- src/controller/c/robot.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 60bf92854e7..17dbee32ed6 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -834,10 +834,14 @@ int wb_robot_step_begin(int duration) { motion_step_all(duration); robot_send_request(duration); + robot_mutex_unlock_step(); + return 0; } int wb_robot_step_end() { + robot_mutex_lock_step(); + if (robot.webots_exit == WEBOTS_EXIT_NOW) return -1; From 7a5a38a8a04c7afc3aed48b682b1328c4ea960d2 Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Mon, 10 Jan 2022 18:03:26 +0100 Subject: [PATCH 03/39] Create fields --- src/controller/c/motor.c | 11 +++++++++++ src/webots/nodes/WbMotor.cpp | 3 +++ 2 files changed, 14 insertions(+) diff --git a/src/controller/c/motor.c b/src/controller/c/motor.c index f005ac547f6..bbe2cb9a749 100644 --- a/src/controller/c/motor.c +++ b/src/controller/c/motor.c @@ -53,6 +53,8 @@ typedef struct { WbJointType type; int requested_device_type; int associated_device_tag; + int siblings_size; + WbDeviceTag *siblings; } Motor; static Motor *motor_create() { @@ -87,6 +89,8 @@ static Motor *motor_create() { motor->configured = false; motor->requested_device_type = 0; motor->associated_device_tag = 0; + motor->siblings_size = 0; + motor->siblings = NULL; return motor; } @@ -178,6 +182,12 @@ static void motor_read_answer(WbDevice *d, WbRequest *r) { m->position = request_read_double(r); m->velocity = request_read_double(r); m->multiplier = request_read_double(r); + m->siblings_size = request_read_int32(r); + if (m->siblings_size > 0) { + m->siblings = (WbDeviceTag *)malloc(sizeof(WbDeviceTag) * m->siblings_size); + for (int i = 0; i < m->siblings_size; i++) + m->siblings[i] = request_read_uint16(r); + } m->configured = true; break; case C_MOTOR_FEEDBACK: @@ -194,6 +204,7 @@ static void motor_read_answer(WbDevice *d, WbRequest *r) { static void motor_cleanup(WbDevice *d) { Motor *m = (Motor *)d->pdata; + free(m->siblings); free(m); } diff --git a/src/webots/nodes/WbMotor.cpp b/src/webots/nodes/WbMotor.cpp index 0b10060d6dd..782cb983e98 100644 --- a/src/webots/nodes/WbMotor.cpp +++ b/src/webots/nodes/WbMotor.cpp @@ -617,6 +617,9 @@ void WbMotor::addConfigureToStream(QDataStream &stream) { stream << (double)mTargetPosition; stream << (double)mTargetVelocity; stream << (double)mMultiplier->value(); + stream << (int)mCoupledMotors.size(); + for (int i = 0; i < mCoupledMotors.size(); ++i) + stream << (WbDeviceTag)mCoupledMotors[i]->tag(); mNeedToConfigure = false; } From 82cfc73857364dbdc54eb8c90183f562fe6e4c60 Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Mon, 10 Jan 2022 23:11:19 +0100 Subject: [PATCH 04/39] Remove libcontroller notification --- src/controller/c/motor.c | 25 +++++++++++++++++++++++++ src/webots/nodes/WbMotor.cpp | 2 -- 2 files changed, 25 insertions(+), 2 deletions(-) diff --git a/src/controller/c/motor.c b/src/controller/c/motor.c index bbe2cb9a749..1f321389893 100644 --- a/src/controller/c/motor.c +++ b/src/controller/c/motor.c @@ -240,6 +240,14 @@ void wb_motor_set_position_no_mutex(WbDeviceTag tag, double pos) { if (m) { m->requests[C_MOTOR_SET_POSITION] = 1; m->position = pos; + + for (int i = 0; i < m->siblings_size; ++i) { + Motor *s = motor_get_struct(m->siblings[i]); + if (s) + s->position = isinf(pos) ? pos : pos * s->multiplier; + else + fprintf(stderr, "Error: %s(): invalid sibling in coupling.\n", __FUNCTION__); + } } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); } @@ -278,6 +286,15 @@ void wb_motor_set_velocity(WbDeviceTag tag, double velocity) { } m->requests[C_MOTOR_SET_VELOCITY] = 1; m->velocity = velocity; + + for (int i = 0; i < m->siblings_size; ++i) { + Motor *s = motor_get_struct(m->siblings[i]); + if (s) + s->velocity = velocity * s->multiplier; + else + fprintf(stderr, "Error: %s(): invalid sibling in coupling.\n", __FUNCTION__); + } + } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); robot_mutex_unlock_step(); @@ -349,6 +366,14 @@ void wb_motor_set_force(WbDeviceTag tag, double force) { if (m) { m->requests[C_MOTOR_SET_FORCE] = 1; m->force = force; + + for (int i = 0; i < m->siblings_size; ++i) { + Motor *s = motor_get_struct(m->siblings[i]); + if (s) + s->force = force * s->multiplier; + else + fprintf(stderr, "Error: %s(): invalid sibling in coupling.\n", __FUNCTION__); + } } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); robot_mutex_unlock_step(); diff --git a/src/webots/nodes/WbMotor.cpp b/src/webots/nodes/WbMotor.cpp index 782cb983e98..bd3061d5a0e 100644 --- a/src/webots/nodes/WbMotor.cpp +++ b/src/webots/nodes/WbMotor.cpp @@ -345,7 +345,6 @@ void WbMotor::setTargetPosition(double position) { } mUserControl = false; - mNeedToConfigure = true; // each sibling has to notify libcontroller about velocityControl/positionControl awake(); } @@ -358,7 +357,6 @@ void WbMotor::setVelocity(double velocity) { mTargetVelocity = mTargetVelocity >= 0.0 ? m : -m; } - mNeedToConfigure = true; // each sibling has to notify libcontroller about velocityControl/positionControl awake(); } From 897773cdba350f40e075be63676715915585bdce Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Mon, 10 Jan 2022 23:13:40 +0100 Subject: [PATCH 05/39] Rename --- src/controller/c/motor.c | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/controller/c/motor.c b/src/controller/c/motor.c index 1f321389893..c662b3d8d56 100644 --- a/src/controller/c/motor.c +++ b/src/controller/c/motor.c @@ -53,8 +53,8 @@ typedef struct { WbJointType type; int requested_device_type; int associated_device_tag; - int siblings_size; - WbDeviceTag *siblings; + int couplings_size; + WbDeviceTag *couplings; } Motor; static Motor *motor_create() { @@ -89,8 +89,8 @@ static Motor *motor_create() { motor->configured = false; motor->requested_device_type = 0; motor->associated_device_tag = 0; - motor->siblings_size = 0; - motor->siblings = NULL; + motor->couplings_size = 0; + motor->couplings = NULL; return motor; } @@ -182,11 +182,11 @@ static void motor_read_answer(WbDevice *d, WbRequest *r) { m->position = request_read_double(r); m->velocity = request_read_double(r); m->multiplier = request_read_double(r); - m->siblings_size = request_read_int32(r); - if (m->siblings_size > 0) { - m->siblings = (WbDeviceTag *)malloc(sizeof(WbDeviceTag) * m->siblings_size); - for (int i = 0; i < m->siblings_size; i++) - m->siblings[i] = request_read_uint16(r); + m->couplings_size = request_read_int32(r); + if (m->couplings_size > 0) { + m->couplings = (WbDeviceTag *)malloc(sizeof(WbDeviceTag) * m->couplings_size); + for (int i = 0; i < m->couplings_size; i++) + m->couplings[i] = request_read_uint16(r); } m->configured = true; break; @@ -204,7 +204,7 @@ static void motor_read_answer(WbDevice *d, WbRequest *r) { static void motor_cleanup(WbDevice *d) { Motor *m = (Motor *)d->pdata; - free(m->siblings); + free(m->couplings); free(m); } @@ -241,8 +241,8 @@ void wb_motor_set_position_no_mutex(WbDeviceTag tag, double pos) { m->requests[C_MOTOR_SET_POSITION] = 1; m->position = pos; - for (int i = 0; i < m->siblings_size; ++i) { - Motor *s = motor_get_struct(m->siblings[i]); + for (int i = 0; i < m->couplings_size; ++i) { + Motor *s = motor_get_struct(m->couplings[i]); if (s) s->position = isinf(pos) ? pos : pos * s->multiplier; else @@ -287,8 +287,8 @@ void wb_motor_set_velocity(WbDeviceTag tag, double velocity) { m->requests[C_MOTOR_SET_VELOCITY] = 1; m->velocity = velocity; - for (int i = 0; i < m->siblings_size; ++i) { - Motor *s = motor_get_struct(m->siblings[i]); + for (int i = 0; i < m->couplings_size; ++i) { + Motor *s = motor_get_struct(m->couplings[i]); if (s) s->velocity = velocity * s->multiplier; else @@ -367,8 +367,8 @@ void wb_motor_set_force(WbDeviceTag tag, double force) { m->requests[C_MOTOR_SET_FORCE] = 1; m->force = force; - for (int i = 0; i < m->siblings_size; ++i) { - Motor *s = motor_get_struct(m->siblings[i]); + for (int i = 0; i < m->couplings_size; ++i) { + Motor *s = motor_get_struct(m->couplings[i]); if (s) s->force = force * s->multiplier; else From 43991cecb426980bd1ba3498b62f206990adf6fa Mon Sep 17 00:00:00 2001 From: ad-daniel Date: Tue, 11 Jan 2022 08:30:28 +0100 Subject: [PATCH 06/39] Remove space --- src/controller/c/motor.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/controller/c/motor.c b/src/controller/c/motor.c index c662b3d8d56..3653e138a3b 100644 --- a/src/controller/c/motor.c +++ b/src/controller/c/motor.c @@ -294,7 +294,6 @@ void wb_motor_set_velocity(WbDeviceTag tag, double velocity) { else fprintf(stderr, "Error: %s(): invalid sibling in coupling.\n", __FUNCTION__); } - } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); robot_mutex_unlock_step(); From 8d2954c12ba2f8ed0ebcdb6157eee41c05cdb1d5 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 11:39:11 +0100 Subject: [PATCH 07/39] Moved comment in robot.h --- include/controller/c/webots/robot.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/controller/c/webots/robot.h b/include/controller/c/webots/robot.h index affd52b886c..b7d943037f4 100644 --- a/include/controller/c/webots/robot.h +++ b/include/controller/c/webots/robot.h @@ -69,8 +69,8 @@ int wb_robot_init_msvc(); // internally, this function just calls wb_robot_init #define wb_robot_init() (setvbuf(stdout, NULL, _IONBF, 0), setvbuf(stderr, NULL, _IONBF, 0), wb_robot_init_msvc()) #endif -int wb_robot_step_begin(int duration); -int wb_robot_step_end(); // milliseconds +int wb_robot_step_begin(int duration); // milliseconds +int wb_robot_step_end(); int wb_robot_step(int duration); // milliseconds #ifdef __CYGWIN__ // In that case, we need to flush explicitly the stdout/stdin streams otherwise they are buffered From 0b65273de09047607a9cd2c95c8993820e1154d6 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 11:44:09 +0100 Subject: [PATCH 08/39] Update Controller.def --- src/controller/c/Controller.def | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/controller/c/Controller.def b/src/controller/c/Controller.def index cc6da327731..644f0246c52 100644 --- a/src/controller/c/Controller.def +++ b/src/controller/c/Controller.def @@ -285,6 +285,8 @@ wb_robot_set_custom_data wb_robot_set_data wb_robot_set_mode wb_robot_step +wb_robot_step_begin +wb_robot_step_end wb_robot_task_new wb_robot_wait_for_user_input_event wb_robot_window_custom_function From 450005ca5ff97ca6f5b8c6163b41a6b69b9d43e1 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 11:54:00 +0100 Subject: [PATCH 09/39] Update Robot.hpp --- include/controller/cpp/webots/Robot.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/controller/cpp/webots/Robot.hpp b/include/controller/cpp/webots/Robot.hpp index eea84592c92..e1ff2da8ff8 100644 --- a/include/controller/cpp/webots/Robot.hpp +++ b/include/controller/cpp/webots/Robot.hpp @@ -68,6 +68,8 @@ namespace webots { virtual ~Robot(); virtual int step(int duration); + virtual int step_begin(int duration); + virtual int step_end(); UserInputEvent waitForUserInputEvent(UserInputEvent event_type, int timeout); std::string getName() const; std::string getUrdf(std::string prefix = "") const; From b1a410c3aae4de419b1e6130cae27fa20caa34d4 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 12:05:57 +0100 Subject: [PATCH 10/39] Update Robot.cpp --- src/controller/cpp/Robot.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/controller/cpp/Robot.cpp b/src/controller/cpp/Robot.cpp index bd7f1584bc0..a2444762548 100644 --- a/src/controller/cpp/Robot.cpp +++ b/src/controller/cpp/Robot.cpp @@ -95,6 +95,14 @@ int Robot::step(int duration) { return wb_robot_step(duration); } +int Robot::step_begin(int duration) { + return wb_robot_step_begin(duration); +} + +int Robot::step_end() { + return wb_robot_step_end(); +} + Robot::UserInputEvent Robot::waitForUserInputEvent(UserInputEvent event_type, int timeout) { return UserInputEvent(wb_robot_wait_for_user_input_event(WbUserInputEvent(event_type), timeout)); } From b41aa5dfd9f41f6898d934dbca2195c9210be175 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 14:15:39 +0100 Subject: [PATCH 11/39] Update matlab and WbLanguage --- lib/controller/matlab/.gitignore | 4 +++- src/controller/matlab/mgenerate.py | 2 ++ src/controller/python/controller.i | 2 ++ src/webots/core/WbLanguage.cpp | 2 ++ 4 files changed, 9 insertions(+), 1 deletion(-) diff --git a/lib/controller/matlab/.gitignore b/lib/controller/matlab/.gitignore index e07be7f09a4..fa307652bac 100644 --- a/lib/controller/matlab/.gitignore +++ b/lib/controller/matlab/.gitignore @@ -212,6 +212,8 @@ wb_robot_get_number_of_devices.m wb_robot_get_device_by_index.m wb_robot_get_type.m wb_robot_step.m +wb_robot_step_begin.m +wb_robot_step_end.m wb_robot_wait_for_user_input_event.m wb_robot_battery_sensor_enable.m wb_robot_battery_sensor_disable.m @@ -437,7 +439,6 @@ WB_MODE_CROSS_COMPILATION.m WB_MODE_REMOTE_CONTROL.m WB_NODE_NO_NODE.m WB_NODE_APPEARANCE.m -WB_NODE_ALTIMETER.m WB_NODE_BACKGROUND.m WB_NODE_BILLBOARD.m WB_NODE_BOX.m @@ -470,6 +471,7 @@ WB_NODE_TRANSFORM.m WB_NODE_VIEWPOINT.m WB_NODE_ROBOT.m WB_NODE_ACCELEROMETER.m +WB_NODE_ALTIMETER.m WB_NODE_BRAKE.m WB_NODE_CAMERA.m WB_NODE_COMPASS.m diff --git a/src/controller/matlab/mgenerate.py b/src/controller/matlab/mgenerate.py index 0e6b7217e66..2b3a468e903 100755 --- a/src/controller/matlab/mgenerate.py +++ b/src/controller/matlab/mgenerate.py @@ -385,6 +385,8 @@ def main(args=None): # gen(PROC, "wb_robot_init()", "robot") # gen(PROC, "wb_robot_cleanup()", "robot") gen(FUNC, "wb_robot_step(duration)", "robot") + gen(FUNC, "wb_robot_step_begin(duration)", "robot") + gen(FUNC, "wb_robot_step_end()", "robot") gen(FUNC, "wb_robot_wait_for_user_input_event(event_type, timeout)", "robot") gen(PROC, "wb_robot_battery_sensor_enable(sampling_period)", "robot") gen(PROC, "wb_robot_battery_sensor_disable()", "robot") diff --git a/src/controller/python/controller.i b/src/controller/python/controller.i index 11224f287a7..8a4e6172bbd 100644 --- a/src/controller/python/controller.i +++ b/src/controller/python/controller.i @@ -84,6 +84,8 @@ using namespace std; //---------------------------------------------------------------------------------------------- %thread webots::Robot::step(int duration); +%thread webots::Robot::step_begin(int duration); +%thread webots::Robot::step_end(); %nothreadblock; //handling std::string diff --git a/src/webots/core/WbLanguage.cpp b/src/webots/core/WbLanguage.cpp index 097c3a2a84c..242b99a38d3 100644 --- a/src/webots/core/WbLanguage.cpp +++ b/src/webots/core/WbLanguage.cpp @@ -336,6 +336,8 @@ static const char *C_API_FUNCTIONS = "wb_accelerometer_enable " "wb_robot_set_custom_data " "wb_robot_set_mode " "wb_robot_step " + "wb_robot_step_begin " + "wb_robot_step_end " "wb_robot_task_new " "wb_robot_wait_for_user_input_event " "wb_robot_window_custom_function " From 08283e52c5efabd889236b31b977f4acce07088e Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 14:27:31 +0100 Subject: [PATCH 12/39] clang-format correction --- include/controller/c/webots/robot.h | 2 +- src/controller/c/robot.c | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/controller/c/webots/robot.h b/include/controller/c/webots/robot.h index b7d943037f4..574e767d67d 100644 --- a/include/controller/c/webots/robot.h +++ b/include/controller/c/webots/robot.h @@ -69,7 +69,7 @@ int wb_robot_init_msvc(); // internally, this function just calls wb_robot_init #define wb_robot_init() (setvbuf(stdout, NULL, _IONBF, 0), setvbuf(stderr, NULL, _IONBF, 0), wb_robot_init_msvc()) #endif -int wb_robot_step_begin(int duration); // milliseconds +int wb_robot_step_begin(int duration); // milliseconds int wb_robot_step_end(); int wb_robot_step(int duration); // milliseconds diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 17dbee32ed6..8434810b543 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -844,7 +844,7 @@ int wb_robot_step_end() { if (robot.webots_exit == WEBOTS_EXIT_NOW) return -1; - + keyboard_step_end(); joystick_step_end(); robot_read_data(); @@ -864,8 +864,8 @@ int wb_robot_step_end() { int wb_robot_step(int duration) { int e = wb_robot_step_begin(duration); - if(e==-1) - return e; + if (e == -1) + return e; e = wb_robot_step_end(); return e; From 61686f4255d1cc78e30e3dfbfac417386ea6703e Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 16:12:19 +0100 Subject: [PATCH 13/39] Updated documentation robot.md --- docs/reference/robot.md | 278 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 278 insertions(+) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index a77d6d48a89..d85566d331e 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -128,6 +128,8 @@ Asynchronous controllers may also be recommended for networked simulations invol ### Robot Functions #### `wb_robot_step` +#### `wb_robot_step_begin` +#### `wb_robot_step_end` #### `wb_robot_init` #### `wb_robot_cleanup` @@ -139,6 +141,8 @@ Asynchronous controllers may also be recommended for networked simulations invol #include int wb_robot_step(int duration); +int wb_robot_step_begin(int duration); +int wb_robot_step_end(); void wb_robot_init(); void wb_robot_cleanup(); ``` @@ -155,6 +159,8 @@ namespace webots { Robot(); virtual ~Robot(); virtual int step(int duration); + virtual int step_begin(int duration); + virtual int step_end(); // ... } } @@ -171,6 +177,8 @@ class Robot: def __init__(self): def __del__(self): def step(self, duration): + def step_begin(self, duration): + def step_end(self): # ... ``` @@ -185,6 +193,8 @@ public class Robot { public Robot(); protected void finalize(); public int step(int duration); + public int step_begin(int duration); + public int step_end(); // ... } ``` @@ -195,6 +205,8 @@ public class Robot { ```MATLAB period = wb_robot_step(duration) +period = wb_robot_step_begin(duration) +period = wb_robot_step_end() ``` %tab-end @@ -240,6 +252,12 @@ It means that the step actually lasted the requested number of milliseconds, but - if `dt` > `duration`, then the actuators values were set at `controller_time` + `dt`, and the sensor values were also measured at `controller_time` + `dt`. It means that the requested step duration could not be respected. +With `wb_robot_step`, the controller code is executed sequentially with the Webots background simulation process. +If the two computational processes (Webots and controller) are slow, it may be interesting to parallelize them. +`wb_robot_step_begin` and `wb_robot_step_end` allow you to do such an implementation. +They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two is executed in parallel to the update of the simulation step. +`wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. + The C API has two additional functions: `wb_robot_init` and `wb_robot_cleanup`. There is no equivalent of the `wb_robot_init` and `wb_robot_cleanup` functions in the Java, Python, C++ and MATLAB APIs. In these languages the necessary initialization and cleanup of the controller library is done automatically. @@ -443,6 +461,266 @@ end %tab-end +%end + +**Advanced controller Example** + +%tab-component "language" + +%tab "C" + +```c +#include +#include +#include + +#define TIME_STEP 32 + +static WbDeviceTag my_sensor, my_led; + +int main() { + /* initialize the webots controller library */ + wb_robot_init(); + + // get device tags + my_sensor = wb_robot_get_device("my_distance_sensor"); + my_led = wb_robot_get_device("my_led"); + + /* enable sensors to read data from them */ + wb_distance_sensor_enable(my_sensor, TIME_STEP); + + /* with a parallelized control loop, it may be necessary to run an initial step to initialize sensor values */ + wb_robot_step(TIME_STEP); + + /* main control loop */ + do { + + /* begin simulation step computation: send command values to Webots for update */ + /* leave the loop when the simulation is over */ + if (wb_robot_step_begin(TIME_STEP) == -1) + break; + + /* the following code (until wb_robot_step_end) is executed in parallel with the background simulation step */ + /* useful for computationally intensive controllers */ + + /* read and process sensor data */ + double val = wb_distance_sensor_get_value(my_sensor); + + /* send actuator commands */ + wb_led_set(my_led, 1); + + /* end simulation step computation: retrieve new sensor values from Webots */ + /* leave the loop when the simulation is over */ + } while (wb_robot_step_end() != -1); + + /* Add here your own exit cleanup code */ + + wb_robot_cleanup(); + + return 0; +} +``` + +%tab-end + +%tab "C++" + +```cpp +#include +#include +#include + +class MyController : public Robot { +public: + MyController() { + timeStep = 32; // set the control time step + + // get device tags + distanceSensor = getDistanceSensor("my_distance_sensor"); + led = getLed("my_led"); + + distanceSensor->enable(timeStep); // enable sensors to read data from them + } + + void run() { + // with a parallelized control loop, it may be necessary to run an initial step to initialize sensor values + step(timeStep); + + // main control loop + do { + // begin simulation step computation: send command values to Webots for update + // leave the loop when the simulation is over + if (step_begin(timeStep) == -1) + break; + + // the following code (until step_end) is executed in parallel with the background simulation step + // useful for computationally intensive controllers + + double val = distanceSensor->getValue(); // Read and process sensor data + led->set(1); // Send actuator commands + + // end simulation step computation: retrieve new sensor values from Webots + // leave the loop when the simulation is over + } while (step_end() != -1); + } + +private: + int timeStep; + DistanceSensor *distanceSensor; + Led *led; +} + +// main C++ program +int main() { + MyController *controller = new MyController(); + controller->run(); + delete controller; + return 0; +} + +``` + +%tab-end + +%tab "Python" + +```python +from controller import Robot + +class MyController(Robot): + def __init__(self): + super(MyController, self).__init__() + self.timeStep = 32 # set the control time step + + # get device tags + self.distanceSensor = self.getDistanceSensor('my_distance_sensor') + self.led = self.getLed('my_led') + self.distanceSensor.enable(timeStep) # enable sensors to read data from them + + def run(self): + # with a parallelized control loop, it may be necessary to run an initial step to initialize sensor values + self.step(self.timeStep) + + # main control loop + while True: + # begin simulation step computation: send command values to Webots for update + # leave the loop when the simulation is over + if self.step_begin(self.timeStep) == -1: + break + + # the following code (until self.step_end) is executed in parallel with the background simulation step + # useful for computationally intensive controllers + + val = self.distanceSensor.getValue() # Read and process sensor data + self.led.set(1) # Send actuator commands + + # end simulation step computation: retrieve new sensor values from Webots + # leave the loop when the simulation is over + if self.step_end() == -1: + break + +# main Python program +controller = MyController() +controller.run() +``` + +%tab-end + +%tab "Java" + +```java +import com.cyberbotics.webots.controller.Robot; +import com.cyberbotics.webots.controller.DistanceSensor; +import com.cyberbotics.webots.controller.Led; + +public class MyController extends Robot { + public MyController() { + timeStep = 32; // set the control time step + + // get device tags + distanceSensor = getDistanceSensor("my_distance_sensor"); + led = getLed("my_led"); + + distanceSensor.enable(timeStep); // enable sensors to read data from them + } + + public void run() { + // with a parallelized control loop, it may be necessary to run an initial step to initialize sensor values + step(timeStep); + + // main control loop + do { + // begin simulation step computation: send command values to Webots for update + // leave the loop when the simulation is over + if(step_begin(timeStep) == -1) + break; + + // the following code (until step_end) is executed in parallel with the background simulation step + // useful for computationally intensive controllers + + double val = distanceSensor.getValue(); // Read and process sensor data + led.set(1); // Send actuator commands + + // end simulation step computation: retrieve new sensor values from Webots + // leave the loop when the simulation is over + } while (step_end() != -1) + } + + private int timeStep; + private DistanceSensor distanceSensor; + private Led led; + + // main Java program + public static void main(String[] args) { + MyController controller = new MyController(); + controller.run(); + } +} +``` + +%tab-end + +%tab "MATLAB" + +```MATLAB + +TIME_STEP = 32; % control time step + +% get device tags +distanceSensor = wb_robot_get_device("my_distance_sensor"); +led = wb_robot_get_device("my_led"); + +% enable sensors to read data from them +wb_distance_sensor_enable(distanceSensor, TIME_STEP); + +% with a parallelized control loop, it may be necessary to run an initial step to initialize sensor values +wb_robot_step(TIME_STEP); + +% main control loop +while 1 + % begin simulation step computation: send command values to Webots for update + % leave the loop when the simulation is over + if wb_robot_step_begin(TIME_STEP) == -1 + break; + end + + % the following code (until step_end) is executed in parallel with the background simulation step + % useful for computationally intensive controllers + + val = wb_distance_sensor_get_value(distanceSensor); % Read and process sensor data + wb_led_set(led, 1); % Send actuator commands + + % end simulation step computation: retrieve new sensor values from Webots + % leave the loop when the simulation is over + if wb_robot_step_end() == -1 + break; + end +end +``` + +%tab-end + + %end --- From 815f5d1fcfb71c8307d95ab3930a08f265d30a2b Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 16:54:35 +0100 Subject: [PATCH 14/39] Added sample usage demo --- docs/reference/robot.md | 2 +- .../parallel_robot_step/.gitignore | 1 + .../controllers/parallel_robot_step/Makefile | 25 ++++ .../parallel_robot_step/parallel_robot_step.c | 132 ++++++++++++++++++ .../worlds/.parallel_robot_step.wbproj | 9 ++ .../worlds/parallel_robot_step.wbt | 17 +++ 6 files changed, 185 insertions(+), 1 deletion(-) create mode 100644 projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore create mode 100644 projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/Makefile create mode 100644 projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c create mode 100644 projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj create mode 100644 projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt diff --git a/docs/reference/robot.md b/docs/reference/robot.md index d85566d331e..812d06b68ce 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -704,7 +704,7 @@ while 1 break; end - % the following code (until step_end) is executed in parallel with the background simulation step + % the following code (until wb_robot_step_end) is executed in parallel with the background simulation step % useful for computationally intensive controllers val = wb_distance_sensor_get_value(distanceSensor); % Read and process sensor data diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore new file mode 100644 index 00000000000..f3ed92536d8 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore @@ -0,0 +1 @@ +/parallel_robot_step diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/Makefile b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/Makefile new file mode 100644 index 00000000000..21c889fdcf7 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/Makefile @@ -0,0 +1,25 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Webots Makefile system +# +# You may add some variable definitions hereafter to customize the build process +# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include + + +# Do not modify the following: this includes Webots global Makefile.include +null := +space := $(null) $(null) +WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c new file mode 100644 index 00000000000..d6b220e6d64 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c @@ -0,0 +1,132 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#define WHEEL_RADIUS 0.02 +#define AXLE_LENGTH 0.052 +#define RANGE (1024 / 2) + +static void compute_odometry(WbDeviceTag left_position_sensor, WbDeviceTag right_position_sensor) { + double l = wb_position_sensor_get_value(left_position_sensor); + double r = wb_position_sensor_get_value(right_position_sensor); + double dl = l * WHEEL_RADIUS; // distance covered by left wheel in meter + double dr = r * WHEEL_RADIUS; // distance covered by right wheel in meter + double da = (dr - dl) / AXLE_LENGTH; // delta orientation + printf("estimated distance covered by left wheel: %g m.\n", dl); + printf("estimated distance covered by right wheel: %g m.\n", dr); + printf("estimated change of orientation: %g rad.\n", da); +} + +int main(int argc, char *argv[]) { + /* define variables */ + WbDeviceTag distance_sensor[8], left_motor, right_motor, left_position_sensor, right_position_sensor; + int i, j; + double speed[2]; + double sensors_value[8]; + double braitenberg_coefficients[8][2] = {{0.942, -0.22}, {0.63, -0.1}, {0.5, -0.06}, {-0.06, -0.06}, + {-0.06, -0.06}, {-0.06, 0.5}, {-0.19, 0.63}, {-0.13, 0.942}}; + int time_step; + int camera_time_step; + /* initialize Webots */ + wb_robot_init(); + + if (strcmp(wb_robot_get_model(), "GCtronic e-puck2") == 0) { + printf("e-puck2 robot\n"); + time_step = 64; + camera_time_step = 64; + } else { // original e-puck + printf("e-puck robot\n"); + time_step = 256; + camera_time_step = 1024; + } + + /* get and enable the camera and accelerometer */ + WbDeviceTag camera = wb_robot_get_device("camera"); + wb_camera_enable(camera, camera_time_step); + WbDeviceTag accelerometer = wb_robot_get_device("accelerometer"); + wb_accelerometer_enable(accelerometer, time_step); + + /* get a handler to the motors and set target position to infinity (speed control). */ + left_motor = wb_robot_get_device("left wheel motor"); + right_motor = wb_robot_get_device("right wheel motor"); + wb_motor_set_position(left_motor, INFINITY); + wb_motor_set_position(right_motor, INFINITY); + wb_motor_set_velocity(left_motor, 0.0); + wb_motor_set_velocity(right_motor, 0.0); + + /* get a handler to the position sensors and enable them. */ + left_position_sensor = wb_robot_get_device("left wheel sensor"); + right_position_sensor = wb_robot_get_device("right wheel sensor"); + wb_position_sensor_enable(left_position_sensor, time_step); + wb_position_sensor_enable(right_position_sensor, time_step); + + for (i = 0; i < 8; i++) { + char device_name[4]; + + /* get distance sensors */ + sprintf(device_name, "ps%d", i); + distance_sensor[i] = wb_robot_get_device(device_name); + wb_distance_sensor_enable(distance_sensor[i], time_step); + } + + /* initial step to initialize sensor values */ + /* avoid NULL values on the first sensor reads */ + wb_robot_step(time_step); + + /* main loop: the code written between wb_robot_step_begin and wb_robot_step_end is executed in parallel with the Webots step + * computation */ + do { + /* begin simulation step computation: send command values to Webots for update */ + /* leave the loop when the simulation is over */ + if (wb_robot_step_begin(time_step) == -1) + break; + + /* get sensors values */ + for (i = 0; i < 8; i++) + sensors_value[i] = wb_distance_sensor_get_value(distance_sensor[i]); + const double *a = wb_accelerometer_get_values(accelerometer); + printf("accelerometer values = %0.2f %0.2f %0.2f\n", a[0], a[1], a[2]); + + /* compute odometry and speed values*/ + compute_odometry(left_position_sensor, right_position_sensor); + for (i = 0; i < 2; i++) { + speed[i] = 0.0; + for (j = 0; j < 8; j++) + speed[i] += braitenberg_coefficients[j][i] * (1.0 - (sensors_value[j] / RANGE)); + } + + /* set speed values */ + wb_motor_set_velocity(left_motor, speed[0]); + wb_motor_set_velocity(right_motor, speed[1]); + + /* end simulation step computation: retrieve new sensor values from Webots */ + /* leave the loop when the simulation is over */ + } while (wb_robot_step_end() != -1); + + wb_robot_cleanup(); + + return 0; +} diff --git a/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj b/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj new file mode 100644 index 00000000000..33c7a520dd2 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj @@ -0,0 +1,9 @@ +Webots Project File version R2022b +perspectives: 000000ff00000000fd00000003000000000000032000000388fc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000000000010000023a0000034dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000034d0000008900ffffff000000030000078000000039fc0100000003fb0000000e0043006f006e0073006f006c00650100000000000004a00000000000000000fb0000001a0052006f0062006f0074003a00200065002d007000750063006b01000004a6000001e00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005440000034d00000004000000040000000100000008fc00000000 +simulationViewPerspectives: 000000ff000000010000000200000100000000a80100000002010000000101 +sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 +maximizedDockId: -1 +centralWidgetVisible: 1 +orthographicViewHeight: 1 +textFiles: 0 "controllers/parallel_robot_step/parallel_robot_step.c" +consoles: Console:All:All diff --git a/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt b/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt new file mode 100644 index 00000000000..96a8b529eba --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt @@ -0,0 +1,17 @@ +#VRML_SIM R2022b utf8 +WorldInfo { +} +Viewpoint { + orientation 0.16978651703738604 -0.44169400145505516 -0.8809534310682574 5.46028959946699 + position -1.4624949595640053 -1.2962285084960832 0.8343563494141362 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +RectangleArena { + floorSize 2 2 +} +E-puck { + controller "parallel_robot_step" +} From 1459efa1f73d22955a473bf84a3d1b0413530725 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 17:30:56 +0100 Subject: [PATCH 15/39] Added API tests --- .../robot_multiple_step_advanced/.gitignore | 1 + .../robot_multiple_step_advanced/Makefile | 25 +++++ .../robot_multiple_step_advanced.c | 102 +++++++++++++++++ .../worlds/robot_multiple_step_advanced.wbt | 104 ++++++++++++++++++ 4 files changed, 232 insertions(+) create mode 100644 tests/api/controllers/robot_multiple_step_advanced/.gitignore create mode 100644 tests/api/controllers/robot_multiple_step_advanced/Makefile create mode 100644 tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c create mode 100644 tests/api/worlds/robot_multiple_step_advanced.wbt diff --git a/tests/api/controllers/robot_multiple_step_advanced/.gitignore b/tests/api/controllers/robot_multiple_step_advanced/.gitignore new file mode 100644 index 00000000000..a7d54cfab7d --- /dev/null +++ b/tests/api/controllers/robot_multiple_step_advanced/.gitignore @@ -0,0 +1 @@ +/robot_multiple_step_advanced diff --git a/tests/api/controllers/robot_multiple_step_advanced/Makefile b/tests/api/controllers/robot_multiple_step_advanced/Makefile new file mode 100644 index 00000000000..21c889fdcf7 --- /dev/null +++ b/tests/api/controllers/robot_multiple_step_advanced/Makefile @@ -0,0 +1,25 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Webots Makefile system +# +# You may add some variable definitions hereafter to customize the build process +# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include + + +# Do not modify the following: this includes Webots global Makefile.include +null := +space := $(null) $(null) +WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c b/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c new file mode 100644 index 00000000000..dfee8815e28 --- /dev/null +++ b/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c @@ -0,0 +1,102 @@ +#include +#include +#include +#include +#include +#include + +#include "../../../lib/ts_assertion.h" +#include "../../../lib/ts_utils.h" + +int main(int argc, char **argv) { + const unsigned char *image = NULL; + int r, g, b; + const double *values = NULL; + + ts_setup(argv[0]); + + int time_step = wb_robot_get_basic_time_step(); + WbDeviceTag camera = wb_robot_get_device("camera"); + WbDeviceTag compass = wb_robot_get_device("compass"); + WbDeviceTag gps = wb_robot_get_device("gps"); + WbDeviceTag ds = wb_robot_get_device("distance sensor"); + WbDeviceTag ts = wb_robot_get_device("touch sensor"); + + wb_camera_enable(camera, 3 * time_step); + wb_compass_enable(compass, 3 * time_step); + wb_gps_enable(gps, 3 * time_step); + wb_distance_sensor_enable(ds, 3 * time_step); + wb_touch_sensor_enable(ts, 3 * time_step); + + // step 1 - 32 ms + // step 2 - 64 ms + // step 3 - 96 ms -> refresh sensor value + // change robot position + // step 4 - 128 ms -> sensor values not updated + // step 5 - 160 ms + // step 6 - 192 ms -> sensor values updated but waiting for step_end + wb_robot_step(time_step); + wb_robot_step_begin(4 * time_step); + + // check initial sensor values + + // camera + image = wb_camera_get_image(camera); + r = wb_camera_image_get_red(image, 1, 0, 0); + g = wb_camera_image_get_green(image, 1, 0, 0); + b = wb_camera_image_get_blue(image, 1, 0, 0); + ts_assert_color_in_delta(r, g, b, 0x66, 0xB2, 0xFF, 5, "Initial camera measurement is wrong."); + + // compass + values = wb_compass_get_values(compass); + const double initialCompassValues[3] = {1.0, 0.0, 0.0}; + ts_assert_doubles_in_delta(3, values, initialCompassValues, 0.0001, "Initial compass measurement is wrong."); + + // distance sensor + ts_assert_double_in_delta(wb_distance_sensor_get_value(ds), 1000.0, 0.001, "Initial distance sensor measurement is wrong."); + + // gps + values = wb_gps_get_values(gps); + const double initialGpsValues[3] = {0.100000, 0.025178, 0.100000}; + ts_assert_doubles_in_delta(3, values, initialGpsValues, 0.00001, "Initial GPS measurement is wrong."); + + // touch sensor + ts_assert_double_in_delta(wb_touch_sensor_get_value(ts), 0.0, 0.001, "Initial touch sensor measurement is wrong."); + + wb_robot_step_end(); // retrieve sensor values after step update + + // check sensor values after robot position changed + + // camera + image = wb_camera_get_image(camera); + r = wb_camera_image_get_red(image, 1, 0, 0); + g = wb_camera_image_get_green(image, 1, 0, 0); + b = wb_camera_image_get_blue(image, 1, 0, 0); + ts_assert_color_in_delta(r, g, b, 207, 207, 207, 5, "Camera measurement after robot moved is wrong."); + + // compass + values = wb_compass_get_values(compass); + const double updatedCompassValues[3] = {0.003230, -0.034126, 0.999412}; + ts_assert_doubles_in_delta(3, values, updatedCompassValues, 0.00001, + "Compass measurement after robot moved is wrong (received: {%f, %f, %f}).", values[0], values[1], + values[2]); + + // distance sensor + double value = wb_distance_sensor_get_value(ds); + ts_assert_double_in_delta(value, 666.766, 0.001, "Distance sensor measurement after robot moved is wrong (received: %f).", + value); + + // gps + values = wb_gps_get_values(gps); + const double updatedGpsValues[3] = {-0.049331, 0.025861, 0.101667}; + ts_assert_doubles_in_delta(3, values, updatedGpsValues, 0.00001, + "GPS measurement after robot moved is wrong (received: {%f, %f, %f}).", values[0], values[1], + values[2]); + + // touch sensor + value = wb_touch_sensor_get_value(ts); + ts_assert_double_in_delta(value, 5.442, 0.001, "Touch sensor measurement after robot moved is wrong (received: %f).", value); + + ts_send_success(); + return EXIT_SUCCESS; +} diff --git a/tests/api/worlds/robot_multiple_step_advanced.wbt b/tests/api/worlds/robot_multiple_step_advanced.wbt new file mode 100644 index 00000000000..a9c92ae58f9 --- /dev/null +++ b/tests/api/worlds/robot_multiple_step_advanced.wbt @@ -0,0 +1,104 @@ +#VRML_SIM R2022b utf8 +WorldInfo { + coordinateSystem "NUE" +} +Viewpoint { + orientation 0.5988649619496187 0.7598749517194467 0.2528849839323209 5.09863 + position -0.3184 0.391568 0.295716 +} +Background { + skyColor [ + 0.4 0.7 1 + ] +} +PointLight { + ambientIntensity 1 + intensity 0.4 + location 0 0.5 0 +} +Floor { + rotation 1 0 0 -1.57079632679 + size 1 1 +} +Robot { + controller "robot_multiple_step_supervisor" + supervisor TRUE +} +DEF OBSTACLE Solid { + translation -0.05 0.05 0.01 + children [ + DEF OBSTALCE_GEOM Shape { + appearance Appearance { + } + geometry Box { + size 0.1 0.1 0.05 + } + } + ] + boundingObject USE OBSTALCE_GEOM +} +DEF ROBOT Robot { + translation 0.1 0.0295 0.1 + children [ + DEF BODY Transform { + children [ + Shape { + geometry Cone { + bottomRadius 0.005 + height 0.02 + } + } + ] + rotation 0 1 0 1.57079632679 + } + Camera { + rotation 1 0 0 -1.5708011490495657 + fieldOfView 0.1 + width 1 + height 1 + } + Compass { + } + GPS { + } + DistanceSensor { + } + TouchSensor { + translation 0.067 0 0.01 + rotation 0.5773509358560258 0.577349935856137 0.577349935856137 -2.094395307179586 + children [ + Shape { + appearance Appearance { + material Material { + diffuseColor 0 1 0 + } + } + geometry DEF TS_BO Sphere { + radius 0.005 + } + } + ] + boundingObject USE TS_BO + physics Physics { + } + type "force" + } + TestSuiteEmitter { + } + ] + boundingObject Transform { + translation 0 -0.004 0 + children [ + Box { + size 0.05 0.05 0.05 + } + ] + } + physics Physics { + density -1 + mass 1 + } + controller "robot_multiple_step" +} +TestSuiteSupervisor { +} From feb65b4a95cfb9cd5db7a8e8ed78d465997acc43 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Tue, 11 Jan 2022 17:38:55 +0100 Subject: [PATCH 16/39] Updated API test to sync with sensors --- .../robot_multiple_step_advanced.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c b/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c index dfee8815e28..7570855836d 100644 --- a/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c +++ b/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c @@ -35,8 +35,8 @@ int main(int argc, char **argv) { // step 4 - 128 ms -> sensor values not updated // step 5 - 160 ms // step 6 - 192 ms -> sensor values updated but waiting for step_end - wb_robot_step(time_step); - wb_robot_step_begin(4 * time_step); + wb_robot_step(3 * time_step); + wb_robot_step_begin(3 * time_step); // check initial sensor values From 65480ce3aa003fd160a22fe028e630931d64a0f7 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Wed, 12 Jan 2022 11:24:54 +0100 Subject: [PATCH 17/39] Quick suggestion improvements Co-authored-by: Olivier Michel --- docs/reference/robot.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index 812d06b68ce..bffab2d5fcf 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -159,8 +159,8 @@ namespace webots { Robot(); virtual ~Robot(); virtual int step(int duration); - virtual int step_begin(int duration); - virtual int step_end(); + virtual int stepBegin(int duration); + virtual int stepEnd(); // ... } } @@ -254,8 +254,8 @@ It means that the requested step duration could not be respected. With `wb_robot_step`, the controller code is executed sequentially with the Webots background simulation process. If the two computational processes (Webots and controller) are slow, it may be interesting to parallelize them. -`wb_robot_step_begin` and `wb_robot_step_end` allow you to do such an implementation. -They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two is executed in parallel to the update of the simulation step. +`wb_robot_step_begin` and `wb_robot_step_end` allow you to achieve such an implementation. +They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. `wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. The C API has two additional functions: `wb_robot_init` and `wb_robot_cleanup`. @@ -463,7 +463,7 @@ end %end -**Advanced controller Example** +**Parallel controller example** %tab-component "language" @@ -500,7 +500,7 @@ int main() { if (wb_robot_step_begin(TIME_STEP) == -1) break; - /* the following code (until wb_robot_step_end) is executed in parallel with the background simulation step */ + /* the following code (until wb_robot_step_end) is executed in parallel with the Webots simulation step */ /* useful for computationally intensive controllers */ /* read and process sensor data */ From 56826f4aba880c7403dda1f3e3f6bbee55401bea Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 11:39:42 +0100 Subject: [PATCH 18/39] Improved documentation and CamelCase correction --- docs/reference/robot.md | 46 ++++++++++++++++--------- include/controller/cpp/webots/Robot.hpp | 4 +-- 2 files changed, 31 insertions(+), 19 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index bffab2d5fcf..6179c500c1b 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -177,8 +177,8 @@ class Robot: def __init__(self): def __del__(self): def step(self, duration): - def step_begin(self, duration): - def step_end(self): + def stepBegin(self, duration): + def stepEnd(self): # ... ``` @@ -193,8 +193,8 @@ public class Robot { public Robot(); protected void finalize(); public int step(int duration); - public int step_begin(int duration); - public int step_end(); + public int stepBegin(int duration); + public int stepEnd(); // ... } ``` @@ -252,7 +252,10 @@ It means that the step actually lasted the requested number of milliseconds, but - if `dt` > `duration`, then the actuators values were set at `controller_time` + `dt`, and the sensor values were also measured at `controller_time` + `dt`. It means that the requested step duration could not be respected. -With `wb_robot_step`, the controller code is executed sequentially with the Webots background simulation process. +When using `wb_robot_step()`, the controller code is executed sequentially with the Webots simulation step, i.e., not in parallel. +This is due to the fact that a typical controller reads sensor information, makes some computation, orders motor commands and calls `wb_robot_step()` which actually sends the motor commands and sensors requests and waits until Webots completes a simulation step, which may take some time depending on the complexity of the simulation. +During this time, the controller is idle, waiting for Webots to complete its simulation step. +On the other hand, prior to starting a new step, Webots waits for all the controllers to send their `wb_robot_step()` messages which may induce some idle waiting time in Webots if a controller doesn't send quickly enough its `wb_robot_step()` message because it is busy with some computation. If the two computational processes (Webots and controller) are slow, it may be interesting to parallelize them. `wb_robot_step_begin` and `wb_robot_step_end` allow you to achieve such an implementation. They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. @@ -274,7 +277,7 @@ In this case the simulation will remain blocked (sleeping) on the current step ( Note that the call to the `wb_robot_cleanup` function must be the last API function call in a C controller. Any subsequent Webots API function call will give unpredictable results. -**Simple controller Example** +**Simple controller example** %tab-component "language" @@ -501,11 +504,12 @@ int main() { break; /* the following code (until wb_robot_step_end) is executed in parallel with the Webots simulation step */ - /* useful for computationally intensive controllers */ /* read and process sensor data */ double val = wb_distance_sensor_get_value(my_sensor); + /* Intensive computation could take place here */ + /* send actuator commands */ wb_led_set(my_led, 1); @@ -550,18 +554,20 @@ public: do { // begin simulation step computation: send command values to Webots for update // leave the loop when the simulation is over - if (step_begin(timeStep) == -1) + if (stepBegin(timeStep) == -1) break; // the following code (until step_end) is executed in parallel with the background simulation step - // useful for computationally intensive controllers double val = distanceSensor->getValue(); // Read and process sensor data + + // Intensive computation could take place here + led->set(1); // Send actuator commands // end simulation step computation: retrieve new sensor values from Webots // leave the loop when the simulation is over - } while (step_end() != -1); + } while (stepEnd() != -1); } private: @@ -605,18 +611,20 @@ class MyController(Robot): while True: # begin simulation step computation: send command values to Webots for update # leave the loop when the simulation is over - if self.step_begin(self.timeStep) == -1: + if self.stepBegin(self.timeStep) == -1: break # the following code (until self.step_end) is executed in parallel with the background simulation step - # useful for computationally intensive controllers val = self.distanceSensor.getValue() # Read and process sensor data + + # Intensive computation could take place here + self.led.set(1) # Send actuator commands # end simulation step computation: retrieve new sensor values from Webots # leave the loop when the simulation is over - if self.step_end() == -1: + if self.stepEnd() == -1: break # main Python program @@ -652,18 +660,20 @@ public class MyController extends Robot { do { // begin simulation step computation: send command values to Webots for update // leave the loop when the simulation is over - if(step_begin(timeStep) == -1) + if(stepBegin(timeStep) == -1) break; // the following code (until step_end) is executed in parallel with the background simulation step - // useful for computationally intensive controllers double val = distanceSensor.getValue(); // Read and process sensor data + + // Intensive computation could take place here + led.set(1); // Send actuator commands // end simulation step computation: retrieve new sensor values from Webots // leave the loop when the simulation is over - } while (step_end() != -1) + } while (stepEnd() != -1) } private int timeStep; @@ -705,9 +715,11 @@ while 1 end % the following code (until wb_robot_step_end) is executed in parallel with the background simulation step - % useful for computationally intensive controllers val = wb_distance_sensor_get_value(distanceSensor); % Read and process sensor data + + % Intensive computation could take place here + wb_led_set(led, 1); % Send actuator commands % end simulation step computation: retrieve new sensor values from Webots diff --git a/include/controller/cpp/webots/Robot.hpp b/include/controller/cpp/webots/Robot.hpp index e1ff2da8ff8..e75b46405c2 100644 --- a/include/controller/cpp/webots/Robot.hpp +++ b/include/controller/cpp/webots/Robot.hpp @@ -68,8 +68,8 @@ namespace webots { virtual ~Robot(); virtual int step(int duration); - virtual int step_begin(int duration); - virtual int step_end(); + virtual int stepBegin(int duration); + virtual int stepEnd(); UserInputEvent waitForUserInputEvent(UserInputEvent event_type, int timeout); std::string getName() const; std::string getUrdf(std::string prefix = "") const; From 7d7c7adc960b97894f14b5320867bea098095dd0 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 11:59:20 +0100 Subject: [PATCH 19/39] Warning in case of incorrect API usage --- src/controller/c/robot.c | 14 ++++++++++++++ src/controller/cpp/Robot.cpp | 4 ++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 8434810b543..7e1a39aa860 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -112,6 +112,8 @@ static WbMutexRef robot_step_mutex; static double simulation_time = 0.0; static unsigned int current_step_duration = 0; static bool should_abort_simulation_waiting = false; +static bool waiting_for_step_begin = false; +static bool waiting_for_step_end = false; // Static functions static void init_robot_window_library() { @@ -796,6 +798,9 @@ void wbr_robot_battery_sensor_set_value(double value) { } int wb_robot_step_begin(int duration) { + if(waiting_for_step_end) + fprintf(stderr, "Warning: %s() called multiple times before calling wb_robot_step_end().\n", __FUNCTION__); + if (!robot.client_exit) html_robot_window_step(duration); @@ -836,10 +841,16 @@ int wb_robot_step_begin(int duration) { robot_mutex_unlock_step(); + waiting_for_step_begin = false; + waiting_for_step_end = true; + return 0; } int wb_robot_step_end() { + if(waiting_for_step_begin) + fprintf(stderr, "Warning: %s() called multiple times before calling wb_robot_step_begin().\n", __FUNCTION__); + robot_mutex_lock_step(); if (robot.webots_exit == WEBOTS_EXIT_NOW) @@ -859,6 +870,9 @@ int wb_robot_step_end() { robot_mutex_unlock_step(); robot_window_read_sensors(); + waiting_for_step_begin = true; + waiting_for_step_end = false; + return e; } diff --git a/src/controller/cpp/Robot.cpp b/src/controller/cpp/Robot.cpp index a2444762548..861da620004 100644 --- a/src/controller/cpp/Robot.cpp +++ b/src/controller/cpp/Robot.cpp @@ -95,11 +95,11 @@ int Robot::step(int duration) { return wb_robot_step(duration); } -int Robot::step_begin(int duration) { +int Robot::stepBegin(int duration) { return wb_robot_step_begin(duration); } -int Robot::step_end() { +int Robot::stepEnd() { return wb_robot_step_end(); } From d7169cab52a4e28b2be7f6d38f091dc4eea0e90c Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 12:35:37 +0100 Subject: [PATCH 20/39] Update sample demo with image computation --- .../parallel_robot_step/.gitignore | 1 + .../parallel_robot_step/parallel_robot_step.c | 190 ++++++----- .../worlds/.parallel_robot_step.wbproj | 4 +- .../worlds/parallel_robot_step.wbt | 297 +++++++++++++++++- 4 files changed, 410 insertions(+), 82 deletions(-) diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore index f3ed92536d8..87c8b8089d8 100644 --- a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore @@ -1 +1,2 @@ /parallel_robot_step +/*.png diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c index d6b220e6d64..1a77c27bc72 100644 --- a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c @@ -14,60 +14,51 @@ * limitations under the License. */ +/* + * Description: An example of the parallelization of the control loop with the Webots simulation step (main code taken from the + * /projects/samples/devices/controllers/camera example). + */ + #include +#include #include - -#include #include -#include -#include #include -#include #include - -#define WHEEL_RADIUS 0.02 -#define AXLE_LENGTH 0.052 -#define RANGE (1024 / 2) - -static void compute_odometry(WbDeviceTag left_position_sensor, WbDeviceTag right_position_sensor) { - double l = wb_position_sensor_get_value(left_position_sensor); - double r = wb_position_sensor_get_value(right_position_sensor); - double dl = l * WHEEL_RADIUS; // distance covered by left wheel in meter - double dr = r * WHEEL_RADIUS; // distance covered by right wheel in meter - double da = (dr - dl) / AXLE_LENGTH; // delta orientation - printf("estimated distance covered by left wheel: %g m.\n", dl); - printf("estimated distance covered by right wheel: %g m.\n", dr); - printf("estimated change of orientation: %g rad.\n", da); -} - -int main(int argc, char *argv[]) { - /* define variables */ - WbDeviceTag distance_sensor[8], left_motor, right_motor, left_position_sensor, right_position_sensor; +#include + +#define ANSI_COLOR_RED "\x1b[31m" +#define ANSI_COLOR_GREEN "\x1b[32m" +#define ANSI_COLOR_YELLOW "\x1b[33m" +#define ANSI_COLOR_BLUE "\x1b[34m" +#define ANSI_COLOR_MAGENTA "\x1b[35m" +#define ANSI_COLOR_CYAN "\x1b[36m" +#define ANSI_COLOR_RESET "\x1b[0m" + +#define SPEED 4 +enum BLOB_TYPE { RED, GREEN, BLUE, NONE }; + +int main() { + WbDeviceTag camera, left_motor, right_motor; + int width, height; + int pause_counter = 0; + int left_speed, right_speed; int i, j; - double speed[2]; - double sensors_value[8]; - double braitenberg_coefficients[8][2] = {{0.942, -0.22}, {0.63, -0.1}, {0.5, -0.06}, {-0.06, -0.06}, - {-0.06, -0.06}, {-0.06, 0.5}, {-0.19, 0.63}, {-0.13, 0.942}}; - int time_step; - int camera_time_step; - /* initialize Webots */ + int red, blue, green; + const char *color_names[3] = {"red", "green", "blue"}; + const char *ansi_colors[3] = {ANSI_COLOR_RED, ANSI_COLOR_GREEN, ANSI_COLOR_BLUE}; + const char *filenames[3] = {"red_blob.png", "green_blob.png", "blue_blob.png"}; + enum BLOB_TYPE current_blob; + wb_robot_init(); - if (strcmp(wb_robot_get_model(), "GCtronic e-puck2") == 0) { - printf("e-puck2 robot\n"); - time_step = 64; - camera_time_step = 64; - } else { // original e-puck - printf("e-puck robot\n"); - time_step = 256; - camera_time_step = 1024; - } - - /* get and enable the camera and accelerometer */ - WbDeviceTag camera = wb_robot_get_device("camera"); - wb_camera_enable(camera, camera_time_step); - WbDeviceTag accelerometer = wb_robot_get_device("accelerometer"); - wb_accelerometer_enable(accelerometer, time_step); + const int time_step = wb_robot_get_basic_time_step(); + + /* Get the camera device, enable it, and store its width and height */ + camera = wb_robot_get_device("camera"); + wb_camera_enable(camera, time_step); + width = wb_camera_get_width(camera); + height = wb_camera_get_height(camera); /* get a handler to the motors and set target position to infinity (speed control). */ left_motor = wb_robot_get_device("left wheel motor"); @@ -77,21 +68,6 @@ int main(int argc, char *argv[]) { wb_motor_set_velocity(left_motor, 0.0); wb_motor_set_velocity(right_motor, 0.0); - /* get a handler to the position sensors and enable them. */ - left_position_sensor = wb_robot_get_device("left wheel sensor"); - right_position_sensor = wb_robot_get_device("right wheel sensor"); - wb_position_sensor_enable(left_position_sensor, time_step); - wb_position_sensor_enable(right_position_sensor, time_step); - - for (i = 0; i < 8; i++) { - char device_name[4]; - - /* get distance sensors */ - sprintf(device_name, "ps%d", i); - distance_sensor[i] = wb_robot_get_device(device_name); - wb_distance_sensor_enable(distance_sensor[i], time_step); - } - /* initial step to initialize sensor values */ /* avoid NULL values on the first sensor reads */ wb_robot_step(time_step); @@ -104,23 +80,85 @@ int main(int argc, char *argv[]) { if (wb_robot_step_begin(time_step) == -1) break; - /* get sensors values */ - for (i = 0; i < 8; i++) - sensors_value[i] = wb_distance_sensor_get_value(distance_sensor[i]); - const double *a = wb_accelerometer_get_values(accelerometer); - printf("accelerometer values = %0.2f %0.2f %0.2f\n", a[0], a[1], a[2]); - - /* compute odometry and speed values*/ - compute_odometry(left_position_sensor, right_position_sensor); - for (i = 0; i < 2; i++) { - speed[i] = 0.0; - for (j = 0; j < 8; j++) - speed[i] += braitenberg_coefficients[j][i] * (1.0 - (sensors_value[j] / RANGE)); + /* Get the new camera values */ + const unsigned char *image = wb_camera_get_image(camera); + + /* Decrement the pause_counter */ + if (pause_counter > 0) + pause_counter--; + + /* Case 1 */ + if (pause_counter > 640 / time_step) { + left_speed = 0; + right_speed = 0; + } + /* Case 2 */ + else if (pause_counter > 0) { + left_speed = -SPEED; + right_speed = SPEED; + } + /* Case 3 */ + else if (!image) { // image may be NULL if Robot.synchronization is FALSE + left_speed = 0; + right_speed = 0; + } else { // pause_counter == 0 + /* Reset the sums */ + red = 0; + green = 0; + blue = 0; + + /* Intense computation on image executed in parallel with the Webots simulation step */ + for (i = width / 3; i < 2 * width / 3; i++) { + for (j = height / 2; j < 3 * height / 4; j++) { + red += wb_camera_image_get_red(image, width, i, j); + blue += wb_camera_image_get_blue(image, width, i, j); + green += wb_camera_image_get_green(image, width, i, j); + } + } + + /* Detect blob */ + if ((red > 3 * green) && (red > 3 * blue)) + current_blob = RED; + else if ((green > 3 * red) && (green > 3 * blue)) + current_blob = GREEN; + else if ((blue > 3 * red) && (blue > 3 * green)) + current_blob = BLUE; + else + current_blob = NONE; + + /* Case 3a */ + if (current_blob == NONE) { + left_speed = -SPEED; + right_speed = SPEED; + } + /* Case 3b */ + else { + left_speed = 0; + right_speed = 0; + printf("Looks like I found a %s%s%s blob.\n", ansi_colors[current_blob], color_names[current_blob], ANSI_COLOR_RESET); + // compute the file path in the user directory + char *filepath; +#ifdef _WIN32 + const char *user_directory = wbu_system_short_path(wbu_system_getenv("USERPROFILE")); + filepath = (char *)malloc(strlen(user_directory) + 16); + strcpy(filepath, user_directory); + strcat(filepath, "\\"); +#else + const char *user_directory = wbu_system_getenv("HOME"); + filepath = (char *)malloc(strlen(user_directory) + 16); + strcpy(filepath, user_directory); + strcat(filepath, "/"); +#endif + strcat(filepath, filenames[current_blob]); + wb_camera_save_image(camera, filepath, 100); + free(filepath); + pause_counter = 1280 / time_step; + } } - /* set speed values */ - wb_motor_set_velocity(left_motor, speed[0]); - wb_motor_set_velocity(right_motor, speed[1]); + /* Set the motor speeds. */ + wb_motor_set_velocity(left_motor, left_speed); + wb_motor_set_velocity(right_motor, right_speed); /* end simulation step computation: retrieve new sensor values from Webots */ /* leave the loop when the simulation is over */ diff --git a/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj b/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj index 33c7a520dd2..77d7e0d4fe0 100644 --- a/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj +++ b/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj @@ -1,6 +1,6 @@ Webots Project File version R2022b -perspectives: 000000ff00000000fd00000003000000000000032000000388fc0100000003fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e0065005400720065006501000000000000039f0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f00770000000000000003200000000000000000000000010000023a0000034dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000034d0000008900ffffff000000030000078000000039fc0100000003fb0000000e0043006f006e0073006f006c00650100000000000004a00000000000000000fb0000001a0052006f0062006f0074003a00200065002d007000750063006b01000004a6000001e00000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000005440000034d00000004000000040000000100000008fc00000000 -simulationViewPerspectives: 000000ff000000010000000200000100000000a80100000002010000000101 +perspectives: 000000ff00000000fd00000003000000000000006900000388fc0100000002fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007700000000000000006900000000000000000000000100000287000002acfc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002ac0000008900ffffff0000000300000780000000dafc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000004f7000002ac00000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101 sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201 maximizedDockId: -1 centralWidgetVisible: 1 diff --git a/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt b/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt index 96a8b529eba..a8a112c7867 100644 --- a/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt +++ b/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt @@ -1,17 +1,306 @@ #VRML_SIM R2022b utf8 WorldInfo { + info [ + "Example use of a color Camera device." + ] + title "Camera" } Viewpoint { - orientation 0.16978651703738604 -0.44169400145505516 -0.8809534310682574 5.46028959946699 - position -1.4624949595640053 -1.2962285084960832 0.8343563494141362 + orientation -0.17458317664901404 -0.7286632169171916 0.6622466540066693 5.6399536822144265 + position -1.298765515155383 0.6306794765789305 0.904897417937484 } TexturedBackground { } TexturedBackgroundLight { } RectangleArena { - floorSize 2 2 } -E-puck { +PointLight { + attenuation 0 0 1 + intensity 0.6 + location 0 0 0.6 +} +DEF GREEN_BOX Solid { + translation 0.25 0.05 0.05 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 1 0 + roughness 0.2189099510000001 + metalness 0 + } + geometry DEF BOX0 Box { + size 0.1 0.23 0.1 + } + } + ] + name "green box" + boundingObject Shape { + appearance PBRAppearance { + metalness 0 + } + geometry USE BOX0 + } +} +DEF BLUE_BOX Solid { + translation -0.27 -0.2 0.05 + rotation 0 0 1 0.31 + children [ + Shape { + appearance PBRAppearance { + baseColor 0 0 1 + roughness 0.519341125 + metalness 0 + } + geometry DEF BOX1 Box { + size 0.1 0.2 0.1 + } + } + ] + name "blue box" + boundingObject Shape { + appearance PBRAppearance { + metalness 0 + } + geometry USE BOX1 + } +} +DEF WHITE_BOX Solid { + translation -0.15 0.2 0.05 + rotation 0 0 1 0.4 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.74902 0.74902 0.74902 + roughness 0.10196099999999997 + metalness 0 + } + geometry DEF BOX2 Box { + size 0.3 0.05 0.1 + } + } + ] + name "gray box" + boundingObject Shape { + appearance PBRAppearance { + metalness 0 + } + geometry USE BOX2 + } +} +DEF RED_BOX Solid { + translation 0.1 -0.42 0.05 + rotation 0 0 1 1.8326 + children [ + Shape { + appearance PBRAppearance { + baseColor 1 0 0 + roughness 0.16827074099999995 + metalness 0 + } + geometry DEF BOX3 Box { + size 0.08 0.2 0.1 + } + } + ] + name "red box" + boundingObject Shape { + appearance PBRAppearance { + metalness 0 + } + geometry USE BOX3 + } +} +Robot { + translation 0 -0.15 0 + rotation 0 0 1 1 + children [ + Transform { + translation 0 0 0.0415 + rotation 0 0 1 -1.570796 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.0820075 0.364731 0.8 + roughness 1 + metalness 0 + } + geometry DEF BODY Cylinder { + height 0.08 + radius 0.045 + } + } + ] + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor 0 0 0.025 + } + device [ + RotationalMotor { + name "left wheel motor" + consumptionFactor 70 + } + PositionSensor { + name "left wheel sensor" + } + ] + endPoint Solid { + translation 0 0.045 0.025 + rotation 0.13448695438559669 -0.13448695438559669 -0.9817466670176033 1.589217 + children [ + DEF WHEEL Transform { + rotation 0.577656895078166 0.5771968951617173 -0.5771968951617173 -2.093935 + children [ + Shape { + appearance PBRAppearance { + baseColor 1 0 0 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.01 + radius 0.025 + } + } + ] + } + ] + name "left wheel" + boundingObject USE WHEEL + physics DEF PHYSICS_WHEEL Physics { + density -1 + mass 0.05 + } + } + } + HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor 0 0 0.025 + } + device [ + RotationalMotor { + name "right wheel motor" + consumptionFactor 70 + } + PositionSensor { + name "right wheel sensor" + } + ] + endPoint Solid { + translation 0 -0.045 0.025 + rotation -0.06235700094427207 0.06235700094427207 -0.9961040150840031 1.5747 + children [ + USE WHEEL + ] + name "right wheel" + boundingObject USE WHEEL + physics USE PHYSICS_WHEEL + } + } + DistanceSensor { + translation 0.042 0.02 0.063 + rotation 0 0 -1 -0.499205 + children [ + DEF SENSOR Transform { + rotation 0.577656895078166 0.5771968951617173 -0.5771968951617173 -2.093935 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.975691 0.981481 0.0252992 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.004 + radius 0.008 + } + } + ] + } + ] + name "ds0" + lookupTable [ + 0 1024 0 + 0.05 1024 0 + 0.15 0 0 + ] + numberOfRays 2 + aperture 1 + } + DistanceSensor { + translation 0.042 -0.02 0.063 + rotation 0 0 1 -0.500795 + children [ + USE SENSOR + ] + name "ds1" + lookupTable [ + 0 1024 0 + 0.05 1024 0 + 0.15 0 0 + ] + numberOfRays 2 + aperture 1 + } + DEF SMILE Transform { + translation 0.036 0 0.025 + rotation 0.25056300542381216 -0.9351130202419243 -0.25056300542381216 1.637834 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.721569 0.290196 0.290196 + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.018 + radius 0.009 + subdivision 3 + } + } + ] + } + Camera { + translation 0.04 0 0.0915 + children [ + Transform { + translation -0.015 0 0 + rotation -0.5771968951617173 -0.577656895078166 -0.5771968951617173 -2.093935 + children [ + Shape { + appearance PBRAppearance { + roughness 1 + metalness 0 + } + geometry Cylinder { + height 0.03 + radius 0.01 + } + } + ] + } + ] + fieldOfView 1.047 + width 1920 + height 1080 + antiAliasing TRUE + noise 0.04 + } + ] + name "MyBot" + boundingObject Transform { + translation 0 0 0.0415 + rotation 0 0 1 -1.570796 + children [ + USE BODY + ] + } + physics Physics { + density -1 + mass 0.5 + } controller "parallel_robot_step" } From 42b150e2b8d66bd0c345b8aa99022308b9d03ebd Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 15:32:48 +0100 Subject: [PATCH 21/39] Improved APi tests --- .../robot_multiple_step_advanced/.gitignore | 1 - .../robot_multiple_step_advanced.c | 102 ----------------- .../robot_parallel_step/.gitignore | 1 + .../Makefile | 0 .../robot_parallel_step/robot_parallel_step.c | 108 ++++++++++++++++++ ...p_advanced.wbt => robot_parallel_step.wbt} | 19 +-- 6 files changed, 115 insertions(+), 116 deletions(-) delete mode 100644 tests/api/controllers/robot_multiple_step_advanced/.gitignore delete mode 100644 tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c create mode 100644 tests/api/controllers/robot_parallel_step/.gitignore rename tests/api/controllers/{robot_multiple_step_advanced => robot_parallel_step}/Makefile (100%) create mode 100644 tests/api/controllers/robot_parallel_step/robot_parallel_step.c rename tests/api/worlds/{robot_multiple_step_advanced.wbt => robot_parallel_step.wbt} (83%) diff --git a/tests/api/controllers/robot_multiple_step_advanced/.gitignore b/tests/api/controllers/robot_multiple_step_advanced/.gitignore deleted file mode 100644 index a7d54cfab7d..00000000000 --- a/tests/api/controllers/robot_multiple_step_advanced/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/robot_multiple_step_advanced diff --git a/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c b/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c deleted file mode 100644 index 7570855836d..00000000000 --- a/tests/api/controllers/robot_multiple_step_advanced/robot_multiple_step_advanced.c +++ /dev/null @@ -1,102 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "../../../lib/ts_assertion.h" -#include "../../../lib/ts_utils.h" - -int main(int argc, char **argv) { - const unsigned char *image = NULL; - int r, g, b; - const double *values = NULL; - - ts_setup(argv[0]); - - int time_step = wb_robot_get_basic_time_step(); - WbDeviceTag camera = wb_robot_get_device("camera"); - WbDeviceTag compass = wb_robot_get_device("compass"); - WbDeviceTag gps = wb_robot_get_device("gps"); - WbDeviceTag ds = wb_robot_get_device("distance sensor"); - WbDeviceTag ts = wb_robot_get_device("touch sensor"); - - wb_camera_enable(camera, 3 * time_step); - wb_compass_enable(compass, 3 * time_step); - wb_gps_enable(gps, 3 * time_step); - wb_distance_sensor_enable(ds, 3 * time_step); - wb_touch_sensor_enable(ts, 3 * time_step); - - // step 1 - 32 ms - // step 2 - 64 ms - // step 3 - 96 ms -> refresh sensor value - // change robot position - // step 4 - 128 ms -> sensor values not updated - // step 5 - 160 ms - // step 6 - 192 ms -> sensor values updated but waiting for step_end - wb_robot_step(3 * time_step); - wb_robot_step_begin(3 * time_step); - - // check initial sensor values - - // camera - image = wb_camera_get_image(camera); - r = wb_camera_image_get_red(image, 1, 0, 0); - g = wb_camera_image_get_green(image, 1, 0, 0); - b = wb_camera_image_get_blue(image, 1, 0, 0); - ts_assert_color_in_delta(r, g, b, 0x66, 0xB2, 0xFF, 5, "Initial camera measurement is wrong."); - - // compass - values = wb_compass_get_values(compass); - const double initialCompassValues[3] = {1.0, 0.0, 0.0}; - ts_assert_doubles_in_delta(3, values, initialCompassValues, 0.0001, "Initial compass measurement is wrong."); - - // distance sensor - ts_assert_double_in_delta(wb_distance_sensor_get_value(ds), 1000.0, 0.001, "Initial distance sensor measurement is wrong."); - - // gps - values = wb_gps_get_values(gps); - const double initialGpsValues[3] = {0.100000, 0.025178, 0.100000}; - ts_assert_doubles_in_delta(3, values, initialGpsValues, 0.00001, "Initial GPS measurement is wrong."); - - // touch sensor - ts_assert_double_in_delta(wb_touch_sensor_get_value(ts), 0.0, 0.001, "Initial touch sensor measurement is wrong."); - - wb_robot_step_end(); // retrieve sensor values after step update - - // check sensor values after robot position changed - - // camera - image = wb_camera_get_image(camera); - r = wb_camera_image_get_red(image, 1, 0, 0); - g = wb_camera_image_get_green(image, 1, 0, 0); - b = wb_camera_image_get_blue(image, 1, 0, 0); - ts_assert_color_in_delta(r, g, b, 207, 207, 207, 5, "Camera measurement after robot moved is wrong."); - - // compass - values = wb_compass_get_values(compass); - const double updatedCompassValues[3] = {0.003230, -0.034126, 0.999412}; - ts_assert_doubles_in_delta(3, values, updatedCompassValues, 0.00001, - "Compass measurement after robot moved is wrong (received: {%f, %f, %f}).", values[0], values[1], - values[2]); - - // distance sensor - double value = wb_distance_sensor_get_value(ds); - ts_assert_double_in_delta(value, 666.766, 0.001, "Distance sensor measurement after robot moved is wrong (received: %f).", - value); - - // gps - values = wb_gps_get_values(gps); - const double updatedGpsValues[3] = {-0.049331, 0.025861, 0.101667}; - ts_assert_doubles_in_delta(3, values, updatedGpsValues, 0.00001, - "GPS measurement after robot moved is wrong (received: {%f, %f, %f}).", values[0], values[1], - values[2]); - - // touch sensor - value = wb_touch_sensor_get_value(ts); - ts_assert_double_in_delta(value, 5.442, 0.001, "Touch sensor measurement after robot moved is wrong (received: %f).", value); - - ts_send_success(); - return EXIT_SUCCESS; -} diff --git a/tests/api/controllers/robot_parallel_step/.gitignore b/tests/api/controllers/robot_parallel_step/.gitignore new file mode 100644 index 00000000000..7b9f2485bb8 --- /dev/null +++ b/tests/api/controllers/robot_parallel_step/.gitignore @@ -0,0 +1 @@ +/robot_parallel_step diff --git a/tests/api/controllers/robot_multiple_step_advanced/Makefile b/tests/api/controllers/robot_parallel_step/Makefile similarity index 100% rename from tests/api/controllers/robot_multiple_step_advanced/Makefile rename to tests/api/controllers/robot_parallel_step/Makefile diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c new file mode 100644 index 00000000000..6e779904568 --- /dev/null +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -0,0 +1,108 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "../../../lib/ts_assertion.h" +#include "../../../lib/ts_utils.h" + +int main(int argc, char **argv) { + const unsigned char *image = NULL; + int r, g, b; + const double *values = NULL; + + ts_setup(argv[0]); + + int time_step = wb_robot_get_basic_time_step(); + WbDeviceTag camera = wb_robot_get_device("camera"); + WbDeviceTag compass = wb_robot_get_device("compass"); + WbDeviceTag gps = wb_robot_get_device("gps"); + WbDeviceTag ds = wb_robot_get_device("distance sensor"); + WbDeviceTag ts = wb_robot_get_device("touch sensor"); + + wb_camera_enable(camera, time_step); + wb_compass_enable(compass, time_step); + wb_gps_enable(gps, time_step); + wb_distance_sensor_enable(ds, time_step); + wb_touch_sensor_enable(ts, time_step); + + // step 1 - robot_step - 32 ms -> get initial sensor values and update position + // step 2 - robot_step_begin and robot_step_end - 64 ms -> check initial camera values + // step 3 - robot_step_begin and robot_step_end - 96 ms -> check camera values after first pose update + wb_robot_step(time_step); + + WbNodeRef robot_node = wb_supervisor_node_get_from_def("ROBOT"); + WbFieldRef robot_trans_field = wb_supervisor_node_get_field(robot_node, "translation"); + WbFieldRef rotationField = wb_supervisor_node_get_field(robot_node, "rotation"); + double newRotation[4] = {0.0, 1.0, 0.0, 1.5708}; + double newTranslation[3] = {-0.05, 0.0295, 0.1}; + wb_supervisor_field_set_sf_rotation(rotationField, newRotation); + wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation); + + // step 2 + wb_robot_step_begin(time_step); + + // check initial sensor values + + // camera + image = wb_camera_get_image(camera); + r = wb_camera_image_get_red(image, 1, 0, 0); + g = wb_camera_image_get_green(image, 1, 0, 0); + b = wb_camera_image_get_blue(image, 1, 0, 0); + + ts_assert_color_in_delta(r, g, b, 102, 178, 255, 5, "Initial camera measurement is wrong."); + + // compass + values = wb_compass_get_values(compass); + const double initialCompassValues[3] = {1.0, 0.0, 0.0}; + ts_assert_doubles_in_delta(3, values, initialCompassValues, 0.0001, "Initial compass measurement is wrong."); + + // distance sensor + ts_assert_double_in_delta(wb_distance_sensor_get_value(ds), 1000.0, 0.001, "Initial distance sensor measurement is wrong."); + + // gps + values = wb_gps_get_values(gps); + const double initialGpsValues[3] = {0.100000, 0.0194546, 0.100000}; + ts_assert_doubles_in_delta(3, values, initialGpsValues, 0.00001, "Initial GPS measurement is wrong."); + + // touch sensor + ts_assert_double_in_delta(wb_touch_sensor_get_value(ts), 0.0, 0.001, "Initial touch sensor measurement is wrong."); + + // second pose update + double newRotation1[4] = {0.0, 0.0, 1.0, 0.0}; + double newTranslation1[3] = {0.1, 0.0295, 0.1}; + wb_supervisor_field_set_sf_rotation(rotationField, newRotation1); + wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation1); + + wb_robot_step_end(); // retrieve sensor values after step update + + // step 3 - check camera value after first pose update + wb_robot_step_begin(time_step); + + image = wb_camera_get_image(camera); + // Intense computation between begin and end + for (int i = 0; i < 100000000; i++) { + r = wb_camera_image_get_red(image, 1, 0, 0); + g = wb_camera_image_get_blue(image, 1, 0, 0); + b = wb_camera_image_get_green(image, 1, 0, 0); + int mean = (r+g+b)/3; + } + + ts_assert_color_in_delta(r, g, b, 207, 207, 207, 5, "Camera measurement after first pose update is wrong."); + + wb_robot_step_end(); + + // check camera value after second pose update + image = wb_camera_get_image(camera); + r = wb_camera_image_get_red(image, 1, 0, 0); + g = wb_camera_image_get_green(image, 1, 0, 0); + b = wb_camera_image_get_blue(image, 1, 0, 0); + + ts_assert_color_in_delta(r, g, b, 102, 178, 255, 5, "Camera measurement after second pose update is wrong."); + + ts_send_success(); + return EXIT_SUCCESS; +} diff --git a/tests/api/worlds/robot_multiple_step_advanced.wbt b/tests/api/worlds/robot_parallel_step.wbt similarity index 83% rename from tests/api/worlds/robot_multiple_step_advanced.wbt rename to tests/api/worlds/robot_parallel_step.wbt index a9c92ae58f9..2745a1c8736 100644 --- a/tests/api/worlds/robot_multiple_step_advanced.wbt +++ b/tests/api/worlds/robot_parallel_step.wbt @@ -3,8 +3,8 @@ WorldInfo { coordinateSystem "NUE" } Viewpoint { - orientation 0.5988649619496187 0.7598749517194467 0.2528849839323209 5.09863 - position -0.3184 0.391568 0.295716 + orientation 0.9286315153002099 -0.37098708232974925 0.00347757612369663 4.528139423982821 + position -0.4333492851082345 0.279699359957294 0.3018558005363777 } Background { skyColor [ @@ -20,10 +20,6 @@ Floor { rotation 1 0 0 -1.57079632679 size 1 1 } -Robot { - controller "robot_multiple_step_supervisor" - supervisor TRUE -} DEF OBSTACLE Solid { translation -0.05 0.05 0.01 children [ @@ -41,6 +37,7 @@ DEF ROBOT Robot { translation 0.1 0.0295 0.1 children [ DEF BODY Transform { + rotation 0 1 0 1.57079632679 children [ Shape { geometry Cone { @@ -49,10 +46,9 @@ DEF ROBOT Robot { } } ] - rotation 0 1 0 1.57079632679 } Camera { - rotation 1 0 0 -1.5708011490495657 + rotation 1 1.88116e-06 -7.19235e-09 -1.5708053071795867 fieldOfView 0.1 width 1 height 1 @@ -83,8 +79,6 @@ DEF ROBOT Robot { } type "force" } - TestSuiteEmitter { - } ] boundingObject Transform { translation 0 -0.004 0 @@ -98,7 +92,6 @@ DEF ROBOT Robot { density -1 mass 1 } - controller "robot_multiple_step" -} -TestSuiteSupervisor { + controller "robot_parallel_step" + supervisor TRUE } From 5dfb02bd6cbd1babfddd279c04c4e39fa23bfcc4 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 15:41:49 +0100 Subject: [PATCH 22/39] Check for supervisor API functions in tests --- .../robot_parallel_step/robot_parallel_step.c | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c index 6e779904568..564497bad9d 100644 --- a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -29,10 +29,8 @@ int main(int argc, char **argv) { wb_distance_sensor_enable(ds, time_step); wb_touch_sensor_enable(ts, time_step); - // step 1 - robot_step - 32 ms -> get initial sensor values and update position - // step 2 - robot_step_begin and robot_step_end - 64 ms -> check initial camera values - // step 3 - robot_step_begin and robot_step_end - 96 ms -> check camera values after first pose update - wb_robot_step(time_step); + // step 1 - 32 ms -> get initial sensor values, robot node/fields and update position + wb_robot_step_begin(time_step); WbNodeRef robot_node = wb_supervisor_node_get_from_def("ROBOT"); WbFieldRef robot_trans_field = wb_supervisor_node_get_field(robot_node, "translation"); @@ -42,10 +40,10 @@ int main(int argc, char **argv) { wb_supervisor_field_set_sf_rotation(rotationField, newRotation); wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation); - // step 2 - wb_robot_step_begin(time_step); + wb_robot_step_end(); - // check initial sensor values + // step 2 - 64 ms -> check initial sensor values + wb_robot_step_begin(time_step); // camera image = wb_camera_get_image(camera); @@ -79,18 +77,19 @@ int main(int argc, char **argv) { wb_robot_step_end(); // retrieve sensor values after step update - // step 3 - check camera value after first pose update + // step 3 - 96 ms -> check camera values after first pose update (second is only visible by the sensor after the next + // step_end) wb_robot_step_begin(time_step); image = wb_camera_get_image(camera); + // Intense computation between begin and end for (int i = 0; i < 100000000; i++) { r = wb_camera_image_get_red(image, 1, 0, 0); g = wb_camera_image_get_blue(image, 1, 0, 0); b = wb_camera_image_get_green(image, 1, 0, 0); - int mean = (r+g+b)/3; + int mean = (r + g + b) / 3; } - ts_assert_color_in_delta(r, g, b, 207, 207, 207, 5, "Camera measurement after first pose update is wrong."); wb_robot_step_end(); From 988386d8990dff7cd87ce79414796317989c96ff Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 15:52:57 +0100 Subject: [PATCH 23/39] Clang format correction --- src/controller/c/robot.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 7e1a39aa860..e3403005854 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -798,7 +798,7 @@ void wbr_robot_battery_sensor_set_value(double value) { } int wb_robot_step_begin(int duration) { - if(waiting_for_step_end) + if (waiting_for_step_end) fprintf(stderr, "Warning: %s() called multiple times before calling wb_robot_step_end().\n", __FUNCTION__); if (!robot.client_exit) @@ -848,7 +848,7 @@ int wb_robot_step_begin(int duration) { } int wb_robot_step_end() { - if(waiting_for_step_begin) + if (waiting_for_step_begin) fprintf(stderr, "Warning: %s() called multiple times before calling wb_robot_step_begin().\n", __FUNCTION__); robot_mutex_lock_step(); From 69998e06ce77011133e0c3c97e0a4a2cc96f7111 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Wed, 12 Jan 2022 16:55:48 +0100 Subject: [PATCH 24/39] Quick coding style corrections Co-authored-by: Olivier Michel --- docs/reference/robot.md | 16 ++++++++-------- .../robot_parallel_step/robot_parallel_step.c | 10 +++++----- 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index 6179c500c1b..adab2296a4f 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -508,7 +508,7 @@ int main() { /* read and process sensor data */ double val = wb_distance_sensor_get_value(my_sensor); - /* Intensive computation could take place here */ + /* intensive computation could take place here */ /* send actuator commands */ wb_led_set(my_led, 1); @@ -561,7 +561,7 @@ public: double val = distanceSensor->getValue(); // Read and process sensor data - // Intensive computation could take place here + // intensive computation could take place here led->set(1); // Send actuator commands @@ -612,20 +612,20 @@ class MyController(Robot): # begin simulation step computation: send command values to Webots for update # leave the loop when the simulation is over if self.stepBegin(self.timeStep) == -1: - break + break # the following code (until self.step_end) is executed in parallel with the background simulation step val = self.distanceSensor.getValue() # Read and process sensor data - # Intensive computation could take place here + # intensive computation could take place here self.led.set(1) # Send actuator commands # end simulation step computation: retrieve new sensor values from Webots # leave the loop when the simulation is over if self.stepEnd() == -1: - break + break # main Python program controller = MyController() @@ -660,14 +660,14 @@ public class MyController extends Robot { do { // begin simulation step computation: send command values to Webots for update // leave the loop when the simulation is over - if(stepBegin(timeStep) == -1) + if (stepBegin(timeStep) == -1) break; // the following code (until step_end) is executed in parallel with the background simulation step double val = distanceSensor.getValue(); // Read and process sensor data - // Intensive computation could take place here + // intensive computation could take place here led.set(1); // Send actuator commands @@ -718,7 +718,7 @@ while 1 val = wb_distance_sensor_get_value(distanceSensor); % Read and process sensor data - % Intensive computation could take place here + % intensive computation could take place here wb_led_set(led, 1); % Send actuator commands diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c index 564497bad9d..8cdc37a64f2 100644 --- a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -16,7 +16,7 @@ int main(int argc, char **argv) { ts_setup(argv[0]); - int time_step = wb_robot_get_basic_time_step(); + const int time_step = wb_robot_get_basic_time_step(); WbDeviceTag camera = wb_robot_get_device("camera"); WbDeviceTag compass = wb_robot_get_device("compass"); WbDeviceTag gps = wb_robot_get_device("gps"); @@ -35,8 +35,8 @@ int main(int argc, char **argv) { WbNodeRef robot_node = wb_supervisor_node_get_from_def("ROBOT"); WbFieldRef robot_trans_field = wb_supervisor_node_get_field(robot_node, "translation"); WbFieldRef rotationField = wb_supervisor_node_get_field(robot_node, "rotation"); - double newRotation[4] = {0.0, 1.0, 0.0, 1.5708}; - double newTranslation[3] = {-0.05, 0.0295, 0.1}; + const double newRotation[4] = {0.0, 1.0, 0.0, 1.5708}; + const double newTranslation[3] = {-0.05, 0.0295, 0.1}; wb_supervisor_field_set_sf_rotation(rotationField, newRotation); wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation); @@ -70,8 +70,8 @@ int main(int argc, char **argv) { ts_assert_double_in_delta(wb_touch_sensor_get_value(ts), 0.0, 0.001, "Initial touch sensor measurement is wrong."); // second pose update - double newRotation1[4] = {0.0, 0.0, 1.0, 0.0}; - double newTranslation1[3] = {0.1, 0.0295, 0.1}; + const double newRotation1[4] = {0.0, 0.0, 1.0, 0.0}; + const double newTranslation1[3] = {0.1, 0.0295, 0.1}; wb_supervisor_field_set_sf_rotation(rotationField, newRotation1); wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation1); From 713e0a594b07d5ff40d479a49215dd201c3ff06b Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Wed, 12 Jan 2022 17:01:38 +0100 Subject: [PATCH 25/39] Homogeneity corrections --- docs/reference/robot.md | 6 ++-- .../parallel_robot_step/parallel_robot_step.c | 28 +++++++++---------- .../robot_parallel_step/robot_parallel_step.c | 2 +- 3 files changed, 18 insertions(+), 18 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index adab2296a4f..4f94f68ba07 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -252,10 +252,10 @@ It means that the step actually lasted the requested number of milliseconds, but - if `dt` > `duration`, then the actuators values were set at `controller_time` + `dt`, and the sensor values were also measured at `controller_time` + `dt`. It means that the requested step duration could not be respected. -When using `wb_robot_step()`, the controller code is executed sequentially with the Webots simulation step, i.e., not in parallel. -This is due to the fact that a typical controller reads sensor information, makes some computation, orders motor commands and calls `wb_robot_step()` which actually sends the motor commands and sensors requests and waits until Webots completes a simulation step, which may take some time depending on the complexity of the simulation. +When using `wb_robot_step`, the controller code is executed sequentially with the Webots simulation step, i.e., not in parallel. +This is due to the fact that a typical controller reads sensor information, makes some computation, orders motor commands and calls `wb_robot_step` which actually sends the motor commands and sensors requests and waits until Webots completes a simulation step, which may take some time depending on the complexity of the simulation. During this time, the controller is idle, waiting for Webots to complete its simulation step. -On the other hand, prior to starting a new step, Webots waits for all the controllers to send their `wb_robot_step()` messages which may induce some idle waiting time in Webots if a controller doesn't send quickly enough its `wb_robot_step()` message because it is busy with some computation. +On the other hand, prior to starting a new step, Webots waits for all the controllers to send their `wb_robot_step` messages which may induce some idle waiting time in Webots if a controller doesn't send quickly enough its `wb_robot_step` message because it is busy with some computation. If the two computational processes (Webots and controller) are slow, it may be interesting to parallelize them. `wb_robot_step_begin` and `wb_robot_step_end` allow you to achieve such an implementation. They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. diff --git a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c index 1a77c27bc72..ec58d02f907 100644 --- a/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c @@ -54,7 +54,7 @@ int main() { const int time_step = wb_robot_get_basic_time_step(); - /* Get the camera device, enable it, and store its width and height */ + /* get the camera device, enable it, and store its width and height */ camera = wb_robot_get_device("camera"); wb_camera_enable(camera, time_step); width = wb_camera_get_width(camera); @@ -80,34 +80,34 @@ int main() { if (wb_robot_step_begin(time_step) == -1) break; - /* Get the new camera values */ + /* get the new camera values */ const unsigned char *image = wb_camera_get_image(camera); - /* Decrement the pause_counter */ + /* decrement the pause_counter */ if (pause_counter > 0) pause_counter--; - /* Case 1 */ + /* case 1 */ if (pause_counter > 640 / time_step) { left_speed = 0; right_speed = 0; } - /* Case 2 */ + /* case 2 */ else if (pause_counter > 0) { left_speed = -SPEED; right_speed = SPEED; } - /* Case 3 */ - else if (!image) { // image may be NULL if Robot.synchronization is FALSE + /* case 3 */ + else if (!image) { left_speed = 0; right_speed = 0; - } else { // pause_counter == 0 - /* Reset the sums */ + } else { + /* reset the sums */ red = 0; green = 0; blue = 0; - /* Intense computation on image executed in parallel with the Webots simulation step */ + /* intense computation on image executed in parallel with the Webots simulation step */ for (i = width / 3; i < 2 * width / 3; i++) { for (j = height / 2; j < 3 * height / 4; j++) { red += wb_camera_image_get_red(image, width, i, j); @@ -116,7 +116,7 @@ int main() { } } - /* Detect blob */ + /* detect blob */ if ((red > 3 * green) && (red > 3 * blue)) current_blob = RED; else if ((green > 3 * red) && (green > 3 * blue)) @@ -126,12 +126,12 @@ int main() { else current_blob = NONE; - /* Case 3a */ + /* case 3a */ if (current_blob == NONE) { left_speed = -SPEED; right_speed = SPEED; } - /* Case 3b */ + /* case 3b */ else { left_speed = 0; right_speed = 0; @@ -156,7 +156,7 @@ int main() { } } - /* Set the motor speeds. */ + /* set the motor speeds. */ wb_motor_set_velocity(left_motor, left_speed); wb_motor_set_velocity(right_motor, right_speed); diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c index 8cdc37a64f2..32d3a8ccbe7 100644 --- a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -84,7 +84,7 @@ int main(int argc, char **argv) { image = wb_camera_get_image(camera); // Intense computation between begin and end - for (int i = 0; i < 100000000; i++) { + for (int i = 0; i < 1000000; i++) { r = wb_camera_image_get_red(image, 1, 0, 0); g = wb_camera_image_get_blue(image, 1, 0, 0); b = wb_camera_image_get_green(image, 1, 0, 0); From c117d487cf6a9f2500b2d35dc1a2d47d69ff3741 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 08:30:01 +0100 Subject: [PATCH 26/39] Camel case correction --- tests/api/controllers/robot_parallel_step/robot_parallel_step.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c index 32d3a8ccbe7..3883ae66b13 100644 --- a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -83,7 +83,7 @@ int main(int argc, char **argv) { image = wb_camera_get_image(camera); - // Intense computation between begin and end + // intense computation between begin and end for (int i = 0; i < 1000000; i++) { r = wb_camera_image_get_red(image, 1, 0, 0); g = wb_camera_image_get_blue(image, 1, 0, 0); From c75c73a579cc240c368dcbdbf0ca844a47a34ea6 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 12:22:57 +0100 Subject: [PATCH 27/39] Update API test and flush func for forbidden use cases --- src/controller/c/robot.c | 24 +- src/controller/c/robot_private.h | 2 +- src/controller/c/supervisor.c | 229 +++++++++--------- .../robot_parallel_step/robot_parallel_step.c | 17 +- 4 files changed, 145 insertions(+), 127 deletions(-) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index e3403005854..1edf115fad6 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -877,6 +877,9 @@ int wb_robot_step_end() { } int wb_robot_step(int duration) { + if (waiting_for_step_end) + fprintf(stderr, "Warning: %s() called before calling wb_robot_step_end().\n", __FUNCTION__); + int e = wb_robot_step_begin(duration); if (e == -1) return e; @@ -923,7 +926,7 @@ WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, robot.is_waiting_for_user_input_event = true; robot.user_input_event_type = event_type; robot.user_input_event_timeout = timeout; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); while (robot.is_waiting_for_user_input_event && !robot_is_quitting()) robot_read_data(); @@ -943,20 +946,31 @@ WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, return robot.user_input_event_type; } -void wb_robot_flush_unlocked() { +bool wb_robot_flush_unlocked(const char *func) { + if (func && waiting_for_step_end) { + fprintf( + stderr, + "Warning: %s(): functions with immediate requests to Webots cannot be implemented in-between wb_robot_step_begin() and " + "wb_robot_step_end()!\n", + func); + return 0; + } + if (robot.webots_exit == WEBOTS_EXIT_NOW) { robot_quit(); robot_mutex_unlock_step(); exit(EXIT_SUCCESS); } if (robot.webots_exit == WEBOTS_EXIT_LATER) - return; + return 0; robot.is_immediate_message = true; robot_send_request(0); robot_read_data(); if (robot.webots_exit == WEBOTS_EXIT_NOW) robot.webots_exit = WEBOTS_EXIT_LATER; robot.is_immediate_message = false; + + return 1; } int wb_robot_init_msvc() { @@ -1163,7 +1177,7 @@ void wb_robot_wwi_send(const char *data, int size) { robot_mutex_lock_step(); robot.wwi_message_to_send_size = size; robot.wwi_message_to_send = data; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1196,7 +1210,7 @@ const char *wb_robot_get_urdf(const char *prefix) { robot.urdf_prefix = malloc(strlen(prefix) + 1); strcpy(robot.urdf_prefix, prefix); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot.need_urdf = false; robot_mutex_unlock_step(); diff --git a/src/controller/c/robot_private.h b/src/controller/c/robot_private.h index 3b5671dc122..3c60c982007 100644 --- a/src/controller/c/robot_private.h +++ b/src/controller/c/robot_private.h @@ -35,7 +35,7 @@ #endif int wb_robot_get_step_duration(); -void wb_robot_flush_unlocked(); +bool wb_robot_flush_unlocked(); void robot_write_request(WbDevice *, WbRequest *); void robot_read_answer(WbDevice *, WbRequest *); WbDevice *robot_get_device_with_node(WbDeviceTag tag, WbNodeType node, bool warning); diff --git a/src/controller/c/supervisor.c b/src/controller/c/supervisor.c index 2345b9d32d0..b4105ef0b83 100644 --- a/src/controller/c/supervisor.c +++ b/src/controller/c/supervisor.c @@ -1236,7 +1236,7 @@ static void create_and_append_field_request(WbFieldStruct *f, int action, int in is_field_immediate_message = request->type != SET; // set operations are postponed } -static void field_operation_with_data(WbFieldStruct *f, int action, int index, union WbFieldData data) { +static void field_operation_with_data(WbFieldStruct *f, int action, int index, union WbFieldData data, const char *func) { robot_mutex_lock_step(); WbFieldRequest *r; for (r = field_requests_list_head; r; r = r->next) { @@ -1270,15 +1270,16 @@ static void field_operation_with_data(WbFieldStruct *f, int action, int index, u // pending get request should remain create_and_append_field_request(f, action, index, data, true); if (action != SET) // Only setter can be postponed. The getter, import and remove actions have to be applied immediately. - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(func); + assert(action != GET || sent_field_get_request == NULL); robot_mutex_unlock_step(); } -static void field_operation(WbFieldStruct *f, int action, int index) { +static void field_operation(WbFieldStruct *f, int action, int index, const char *func) { union WbFieldData data; data.sf_string = NULL; - field_operation_with_data(f, action, index, data); + field_operation_with_data(f, action, index, data, func); } static bool check_field(WbFieldRef f, const char *func, WbFieldType type, bool check_type, int *index, bool is_importing, @@ -1439,7 +1440,7 @@ void wb_supervisor_set_label(int id, const char *text, double x, double y, doubl l->y = y; l->size = size; l->color = color_and_transparency; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1456,7 +1457,7 @@ void wb_supervisor_node_save_state(WbNodeRef node, const char *state_name) { robot_mutex_lock_step(); save_node_state_node_ref = node; save_node_state_name = state_name; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); save_node_state_node_ref = NULL; save_node_state_name = NULL; robot_mutex_unlock_step(); @@ -1475,7 +1476,7 @@ void wb_supervisor_node_load_state(WbNodeRef node, const char *state_name) { robot_mutex_lock_step(); reset_node_state_node_ref = node; reset_node_state_name = state_name; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); reset_node_state_node_ref = NULL; reset_node_state_name = NULL; robot_mutex_unlock_step(); @@ -1499,7 +1500,7 @@ void wb_supervisor_export_image(const char *filename, int quality) { free(export_image_filename); export_image_filename = supervisor_strdup(filename); export_image_quality = quality; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1539,7 +1540,7 @@ void wb_supervisor_movie_start_recording(const char *filename, int width, int he movie_quality = quality; movie_acceleration = acceleration; movie_caption = caption; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1549,7 +1550,7 @@ void wb_supervisor_movie_stop_recording() { robot_mutex_lock_step(); movie_stop = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1558,7 +1559,7 @@ bool wb_supervisor_movie_is_ready() { return false; robot_mutex_lock_step(); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return movie_status == WB_SUPERVISOR_MOVIE_READY || movie_status > WB_SUPERVISOR_MOVIE_SAVING; @@ -1569,7 +1570,7 @@ bool wb_supervisor_movie_failed() { return true; robot_mutex_lock_step(); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return movie_status > WB_SUPERVISOR_MOVIE_SAVING; @@ -1627,7 +1628,7 @@ bool wb_supervisor_animation_start_recording(const char *filename) { robot_mutex_lock_step(); free(animation_filename); animation_filename = supervisor_strdup(filename); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return animation_start_status; @@ -1641,7 +1642,7 @@ bool wb_supervisor_animation_stop_recording() { robot_mutex_lock_step(); animation_stop = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return animation_stop_status; @@ -1654,7 +1655,7 @@ void wb_supervisor_simulation_quit(int status) { robot_mutex_lock_step(); simulation_quit = true; simulation_quit_status = status; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1664,7 +1665,7 @@ void wb_supervisor_simulation_reset() { robot_mutex_lock_step(); simulation_reset = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1693,7 +1694,7 @@ void wb_supervisor_simulation_set_mode(WbSimulationMode mode) { robot_mutex_lock_step(); robot_set_simulation_mode(mode); simulation_change_mode = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1703,7 +1704,7 @@ void wb_supervisor_simulation_reset_physics() { robot_mutex_lock_step(); simulation_reset_physics = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1725,7 +1726,7 @@ void wb_supervisor_world_load(const char *filename) { robot_mutex_lock_step(); world_to_load = filename; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1763,7 +1764,7 @@ bool wb_supervisor_world_save(const char *filename) { robot_mutex_lock_step(); save_filename = supervisor_strdup(filename); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return save_status; @@ -1775,7 +1776,7 @@ void wb_supervisor_world_reload() { robot_mutex_lock_step(); world_reload = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -1812,14 +1813,14 @@ int wb_supervisor_node_get_id(WbNodeRef node) { return node->id; } -static WbNodeRef node_get_from_id(int id) { +static WbNodeRef node_get_from_id(int id, const char *func) { robot_mutex_lock_step(); WbNodeRef result = find_node_by_id(id); if (!result) { WbNodeRef node_list_before = node_list; node_id = id; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(func); if (node_list != node_list_before) result = node_list; else @@ -1839,7 +1840,7 @@ WbNodeRef wb_supervisor_node_get_from_id(int id) { return NULL; } - return node_get_from_id(id); + return node_get_from_id(id, __FUNCTION__); } WbNodeRef wb_supervisor_node_get_from_def(const char *def) { @@ -1859,7 +1860,7 @@ WbNodeRef wb_supervisor_node_get_from_def(const char *def) { // otherwise: need to talk to Webots node_def_name = def; node_id = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (node_id >= 0) result = find_node_by_id(node_id); node_def_name = NULL; @@ -1887,7 +1888,7 @@ WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag) { allow_search_in_proto = true; node_tag = tag; node_id = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (node_id >= 0) result = find_node_by_id(node_id); node_tag = -1; @@ -1941,7 +1942,7 @@ WbNodeRef wb_supervisor_node_get_from_proto_def(WbNodeRef node, const char *def) node_def_name = def; node_id = -1; proto_id = node->id; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (node_id >= 0) { result = find_node_by_id(node_id); if (result) { @@ -1968,7 +1969,7 @@ WbNodeRef wb_supervisor_node_get_parent_node(WbNodeRef node) { } allow_search_in_proto = true; - WbNodeRef parent_node = node_get_from_id(node->parent_id); + WbNodeRef parent_node = node_get_from_id(node->parent_id, __FUNCTION__); allow_search_in_proto = false; return parent_node; } @@ -1982,7 +1983,7 @@ WbNodeRef wb_supervisor_node_get_selected() { WbNodeRef result = NULL; node_get_selected = true; node_id = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (node_id >= 0) result = find_node_by_id(node_id); node_id = -1; @@ -2004,7 +2005,7 @@ const double *wb_supervisor_node_get_position(WbNodeRef node) { robot_mutex_lock_step(); position_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); position_node_ref = NULL; robot_mutex_unlock_step(); return node->position ? node->position : invalid_vector; // will be (NaN, NaN, NaN) if n is not derived from Transform @@ -2022,7 +2023,7 @@ const double *wb_supervisor_node_get_orientation(WbNodeRef node) { robot_mutex_lock_step(); orientation_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); orientation_node_ref = NULL; robot_mutex_unlock_step(); return node->orientation ? node->orientation : invalid_vector; // will be (NaN, ..., NaN) if n is not derived from Transform @@ -2059,7 +2060,7 @@ const double *wb_supervisor_node_get_pose(WbNodeRef node, WbNodeRef from_node) { pose_requested = true; pose.from_node = from_node; pose.to_node = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); pose_requested = false; robot_mutex_unlock_step(); return pose.pose; @@ -2077,7 +2078,7 @@ const double *wb_supervisor_node_get_center_of_mass(WbNodeRef node) { robot_mutex_lock_step(); center_of_mass_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); center_of_mass_node_ref = NULL; robot_mutex_unlock_step(); return node->center_of_mass ? node->center_of_mass : invalid_vector; // will be NULL if n is not a Solid @@ -2110,7 +2111,7 @@ const double *wb_supervisor_node_get_contact_point(WbNodeRef node, int index) { robot_mutex_lock_step(); contact_points_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); contact_points_node_ref = NULL; robot_mutex_unlock_step(); @@ -2143,7 +2144,7 @@ WbNodeRef wb_supervisor_node_get_contact_point_node(WbNodeRef node, int index) { node->contact_points_include_descendants = contact_points_include_descendants; robot_mutex_lock_step(); contact_points_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); contact_points_node_ref = NULL; robot_mutex_unlock_step(); } @@ -2151,7 +2152,7 @@ WbNodeRef wb_supervisor_node_get_contact_point_node(WbNodeRef node, int index) { if (!node->contact_points[descendants].points || index >= node->contact_points[descendants].n) return NULL; allow_search_in_proto = true; - WbNodeRef result = node_get_from_id(node->contact_points[descendants].points[index].node_id); + WbNodeRef result = node_get_from_id(node->contact_points[descendants].points[index].node_id, __FUNCTION__); allow_search_in_proto = false; return result; } @@ -2183,7 +2184,7 @@ int wb_supervisor_node_get_number_of_contact_points(WbNodeRef node, bool include robot_mutex_lock_step(); contact_points_node_ref = node; contact_points_include_descendants = include_descendants; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); contact_points_node_ref = NULL; robot_mutex_unlock_step(); @@ -2214,7 +2215,7 @@ WbContactPoint *wb_supervisor_node_get_contact_points(WbNodeRef node, bool inclu robot_mutex_lock_step(); contact_points_node_ref = node; contact_points_include_descendants = include_descendants; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); contact_points_node_ref = NULL; robot_mutex_unlock_step(); @@ -2234,7 +2235,7 @@ bool wb_supervisor_node_get_static_balance(WbNodeRef node) { robot_mutex_lock_step(); static_balance_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); static_balance_node_ref = NULL; robot_mutex_unlock_step(); @@ -2318,7 +2319,7 @@ WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, int index) { WbFieldRef field_list_before = field_list; requested_field_index = index; node_ref = node->id; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); requested_field_index = -1; if (field_list != field_list_before) result = field_list; @@ -2355,7 +2356,7 @@ WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, int index requested_field_index = index; node_ref = node->id; allow_search_in_proto = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); requested_field_index = -1; if (field_list != field_list_before) result = field_list; @@ -2391,7 +2392,7 @@ WbFieldRef wb_supervisor_node_get_field(WbNodeRef node, const char *field_name) // otherwise: need to talk to Webots requested_field_name = field_name; node_ref = node->id; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (requested_field_name) { requested_field_name = NULL; result = field_list; // was just inserted at list head @@ -2417,7 +2418,7 @@ int wb_supervisor_node_get_number_of_fields(WbNodeRef node) { requested_node_number_of_fields = true; node_ref = node->id; node_number_of_fields = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); requested_node_number_of_fields = false; robot_mutex_unlock_step(); if (node_number_of_fields > 0) @@ -2440,7 +2441,7 @@ int wb_supervisor_node_get_proto_number_of_fields(WbNodeRef node) { node_ref = node->id; node_number_of_fields = -1; allow_search_in_proto = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); requested_node_number_of_fields = false; allow_search_in_proto = false; robot_mutex_unlock_step(); @@ -2479,7 +2480,7 @@ WbFieldRef wb_supervisor_node_get_proto_field(WbNodeRef node, const char *field_ requested_field_name = field_name; node_ref = node->id; allow_search_in_proto = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (requested_field_name) { requested_field_name = NULL; result = field_list; // was just inserted at list head @@ -2510,7 +2511,7 @@ void wb_supervisor_node_remove(WbNodeRef node) { robot_mutex_lock_step(); node_to_remove = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -2526,7 +2527,7 @@ const char *wb_supervisor_node_export_string(WbNodeRef node) { robot_mutex_lock_step(); export_string_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); export_string_node_ref = NULL; robot_mutex_unlock_step(); @@ -2547,7 +2548,7 @@ const double *wb_supervisor_node_get_velocity(WbNodeRef node) { free(node->solid_velocity); node->solid_velocity = NULL; get_velocity_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); get_velocity_node_ref = NULL; robot_mutex_unlock_step(); // cppcheck-suppress knownConditionTrueFalse @@ -2570,7 +2571,7 @@ void wb_supervisor_node_set_velocity(WbNodeRef node, const double velocity[6]) { robot_mutex_lock_step(); set_velocity_node_ref = node; solid_velocity = velocity; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); set_velocity_node_ref = NULL; solid_velocity = NULL; robot_mutex_unlock_step(); @@ -2588,7 +2589,7 @@ void wb_supervisor_node_reset_physics(WbNodeRef node) { robot_mutex_lock_step(); reset_physics_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); reset_physics_node_ref = NULL; robot_mutex_unlock_step(); } @@ -2605,7 +2606,7 @@ void wb_supervisor_node_restart_controller(WbNodeRef node) { robot_mutex_lock_step(); restart_controller_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); restart_controller_node_ref = NULL; robot_mutex_unlock_step(); } @@ -2638,7 +2639,7 @@ void wb_supervisor_node_set_visibility(WbNodeRef node, WbNodeRef from, bool visi set_visibility_node_ref = node; set_visibility_from_node_ref = from; node_visible = visible; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); set_visibility_node_ref = NULL; set_visibility_from_node_ref = NULL; robot_mutex_unlock_step(); @@ -2656,7 +2657,7 @@ void wb_supervisor_node_move_viewpoint(WbNodeRef node) { robot_mutex_lock_step(); move_viewpoint_node_ref = node; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); move_viewpoint_node_ref = NULL; robot_mutex_unlock_step(); } @@ -2678,7 +2679,7 @@ void wb_supervisor_node_add_force(WbNodeRef node, const double force[3], bool re add_force_node_ref = node; add_force_or_torque = force; add_force_or_torque_relative = relative; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); add_force_node_ref = NULL; add_force_or_torque = NULL; robot_mutex_unlock_step(); @@ -2705,7 +2706,7 @@ void wb_supervisor_node_add_force_with_offset(WbNodeRef node, const double force add_force_or_torque = force; add_force_offset = offset; add_force_or_torque_relative = relative; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); add_force_with_offset_node_ref = NULL; add_force_or_torque = NULL; add_force_offset = NULL; @@ -2729,7 +2730,7 @@ void wb_supervisor_node_add_torque(WbNodeRef node, const double torque[3], bool add_torque_node_ref = node; add_force_or_torque = torque; add_force_or_torque_relative = relative; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); add_torque_node_ref = NULL; add_force_or_torque = NULL; robot_mutex_unlock_step(); @@ -2787,7 +2788,7 @@ void wb_supervisor_node_set_joint_position(WbNodeRef node, double position, int set_joint_node_ref = node; set_joint_position = position; set_joint_index = index; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); set_joint_node_ref = NULL; robot_mutex_unlock_step(); } @@ -2798,7 +2799,7 @@ bool wb_supervisor_virtual_reality_headset_is_used() { robot_mutex_lock_step(); virtual_reality_headset_is_used_request = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); virtual_reality_headset_is_used_request = false; robot_mutex_unlock_step(); return virtual_reality_headset_is_used; @@ -2812,7 +2813,7 @@ const double *wb_supervisor_virtual_reality_headset_get_position() { virtual_reality_headset_position_request = true; free(virtual_reality_headset_position); virtual_reality_headset_position = NULL; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); virtual_reality_headset_position_request = false; robot_mutex_unlock_step(); return virtual_reality_headset_position ? virtual_reality_headset_position : invalid_vector; @@ -2826,7 +2827,7 @@ const double *wb_supervisor_virtual_reality_headset_get_orientation() { virtual_reality_headset_orientation_request = true; free(virtual_reality_headset_orientation); virtual_reality_headset_orientation = NULL; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); virtual_reality_headset_orientation_request = false; robot_mutex_unlock_step(); return virtual_reality_headset_orientation ? virtual_reality_headset_orientation : invalid_vector; @@ -2880,7 +2881,7 @@ void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampli contact_point_change_tracking.include_descendants = include_descendants; node->contact_points[descendants].last_update = -DBL_MAX; node->contact_points[descendants].sampling_period = sampling_period; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); pose_change_tracking_requested = false; robot_mutex_unlock_step(); } @@ -2899,7 +2900,7 @@ void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool incl contact_point_change_tracking.node = node; contact_point_change_tracking.enable = false; contact_point_change_tracking.include_descendants = include_descendants; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); pose_change_tracking_requested = false; robot_mutex_unlock_step(); } @@ -2918,7 +2919,7 @@ void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_perio field_change_tracking.sampling_period = sampling_period; field_change_tracking.enable = true; field_change_tracking_requested = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); field_change_tracking_requested = false; robot_mutex_unlock_step(); } @@ -2931,7 +2932,7 @@ void wb_supervisor_field_disable_sf_tracking(WbFieldRef field) { field_change_tracking.field = field; field_change_tracking.enable = false; field_change_tracking_requested = true; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); field_change_tracking_requested = false; robot_mutex_unlock_step(); } @@ -2982,7 +2983,7 @@ void wb_supervisor_node_enable_pose_tracking(WbNodeRef node, int sampling_period assert(pose_collection); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); pose_change_tracking_requested = false; robot_mutex_unlock_step(); } @@ -3007,7 +3008,7 @@ void wb_supervisor_node_disable_pose_tracking(WbNodeRef node, WbNodeRef from_nod pose_change_tracking.node = node; pose_change_tracking.from_node = from_node; pose_change_tracking.enable = false; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); pose_change_tracking.node = NULL; robot_mutex_unlock_step(); } @@ -3016,7 +3017,7 @@ bool wb_supervisor_field_get_sf_bool(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_BOOL, true, NULL, false, false)) return false; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_bool; } @@ -3024,7 +3025,7 @@ int wb_supervisor_field_get_sf_int32(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_INT32, true, NULL, false, false)) return 0; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_int32; } @@ -3032,7 +3033,7 @@ double wb_supervisor_field_get_sf_float(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_FLOAT, true, NULL, false, false)) return 0.0; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_float; } @@ -3040,7 +3041,7 @@ const double *wb_supervisor_field_get_sf_vec2f(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_VEC2F, true, NULL, false, false)) return NULL; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec2f; } @@ -3048,7 +3049,7 @@ const double *wb_supervisor_field_get_sf_vec3f(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_VEC3F, true, NULL, false, false)) return NULL; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec3f; } @@ -3056,7 +3057,7 @@ const double *wb_supervisor_field_get_sf_rotation(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_ROTATION, true, NULL, false, false)) return NULL; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_rotation; } @@ -3064,7 +3065,7 @@ const double *wb_supervisor_field_get_sf_color(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_COLOR, true, NULL, false, false)) return NULL; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec3f; } @@ -3072,7 +3073,7 @@ const char *wb_supervisor_field_get_sf_string(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_STRING, true, NULL, false, false)) return ""; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_string; } @@ -3080,7 +3081,7 @@ WbNodeRef wb_supervisor_field_get_sf_node(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_NODE, true, NULL, false, false)) return NULL; - field_operation(field, GET, -1); + field_operation(field, GET, -1, __FUNCTION__); int id = ((WbFieldStruct *)field)->data.sf_node_uid; if (id <= 0) return NULL; @@ -3094,7 +3095,7 @@ bool wb_supervisor_field_get_mf_bool(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_BOOL, true, &index, false, false)) return 0; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_bool; } @@ -3102,7 +3103,7 @@ int wb_supervisor_field_get_mf_int32(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_INT32, true, &index, false, false)) return 0; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_int32; } @@ -3110,7 +3111,7 @@ double wb_supervisor_field_get_mf_float(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_FLOAT, true, &index, false, false)) return 0.0; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_float; } @@ -3118,7 +3119,7 @@ const double *wb_supervisor_field_get_mf_vec2f(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_VEC2F, true, &index, false, false)) return NULL; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec2f; } @@ -3126,7 +3127,7 @@ const double *wb_supervisor_field_get_mf_vec3f(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_VEC3F, true, &index, false, false)) return NULL; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec3f; } @@ -3134,7 +3135,7 @@ const double *wb_supervisor_field_get_mf_color(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_COLOR, true, &index, false, false)) return NULL; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_vec3f; } @@ -3142,7 +3143,7 @@ const double *wb_supervisor_field_get_mf_rotation(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_ROTATION, true, &index, false, false)) return NULL; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_rotation; } @@ -3150,7 +3151,7 @@ const char *wb_supervisor_field_get_mf_string(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_STRING, true, &index, false, false)) return ""; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); return ((WbFieldStruct *)field)->data.sf_string; } @@ -3158,7 +3159,7 @@ WbNodeRef wb_supervisor_field_get_mf_node(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF_NODE, true, &index, false, false)) return NULL; - field_operation(field, GET, index); + field_operation(field, GET, index, __FUNCTION__); WbNodeRef result = find_node_by_id(((WbFieldStruct *)field)->data.sf_node_uid); if (result && ((WbFieldStruct *)field)->is_proto_internal) result->is_proto_internal = true; @@ -3171,7 +3172,7 @@ void wb_supervisor_field_set_sf_bool(WbFieldRef field, bool value) { union WbFieldData data; data.sf_bool = value; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_sf_int32(WbFieldRef field, int value) { @@ -3180,7 +3181,7 @@ void wb_supervisor_field_set_sf_int32(WbFieldRef field, int value) { union WbFieldData data; data.sf_int32 = value; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_sf_float(WbFieldRef field, double value) { @@ -3192,7 +3193,7 @@ void wb_supervisor_field_set_sf_float(WbFieldRef field, double value) { union WbFieldData data; data.sf_float = value; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_sf_vec2f(WbFieldRef field, const double values[2]) { @@ -3205,7 +3206,7 @@ void wb_supervisor_field_set_sf_vec2f(WbFieldRef field, const double values[2]) union WbFieldData data; data.sf_vec2f[0] = values[0]; data.sf_vec2f[1] = values[1]; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_sf_vec3f(WbFieldRef field, const double values[3]) { @@ -3219,7 +3220,7 @@ void wb_supervisor_field_set_sf_vec3f(WbFieldRef field, const double values[3]) data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } static bool isValidRotation(const double r[4]) { @@ -3243,7 +3244,7 @@ void wb_supervisor_field_set_sf_rotation(WbFieldRef field, const double values[4 data.sf_rotation[1] = values[1]; data.sf_rotation[2] = values[2]; data.sf_rotation[3] = values[3]; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } static bool isValidColor(const double rgb[3]) { @@ -3268,7 +3269,7 @@ void wb_supervisor_field_set_sf_color(WbFieldRef field, const double values[3]) data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_sf_string(WbFieldRef field, const char *value) { @@ -3282,7 +3283,7 @@ void wb_supervisor_field_set_sf_string(WbFieldRef field, const char *value) { union WbFieldData data; data.sf_string = supervisor_strdup(value); - field_operation_with_data(field, SET, -1, data); + field_operation_with_data(field, SET, -1, data, __FUNCTION__); } void wb_supervisor_field_set_mf_bool(WbFieldRef field, int index, bool value) { @@ -3291,7 +3292,7 @@ void wb_supervisor_field_set_mf_bool(WbFieldRef field, int index, bool value) { union WbFieldData data; data.sf_bool = value; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_int32(WbFieldRef field, int index, int value) { @@ -3300,7 +3301,7 @@ void wb_supervisor_field_set_mf_int32(WbFieldRef field, int index, int value) { union WbFieldData data; data.sf_int32 = value; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_float(WbFieldRef field, int index, double value) { @@ -3312,7 +3313,7 @@ void wb_supervisor_field_set_mf_float(WbFieldRef field, int index, double value) union WbFieldData data; data.sf_float = value; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_vec2f(WbFieldRef field, int index, const double values[2]) { @@ -3325,7 +3326,7 @@ void wb_supervisor_field_set_mf_vec2f(WbFieldRef field, int index, const double union WbFieldData data; data.sf_vec2f[0] = values[0]; data.sf_vec2f[1] = values[1]; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_vec3f(WbFieldRef field, int index, const double values[3]) { @@ -3339,7 +3340,7 @@ void wb_supervisor_field_set_mf_vec3f(WbFieldRef field, int index, const double data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_rotation(WbFieldRef field, int index, const double values[4]) { @@ -3359,7 +3360,7 @@ void wb_supervisor_field_set_mf_rotation(WbFieldRef field, int index, const doub data.sf_rotation[1] = values[1]; data.sf_rotation[2] = values[2]; data.sf_rotation[3] = values[3]; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_color(WbFieldRef field, int index, const double values[3]) { @@ -3380,7 +3381,7 @@ void wb_supervisor_field_set_mf_color(WbFieldRef field, int index, const double data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_set_mf_string(WbFieldRef field, int index, const char *value) { @@ -3394,7 +3395,7 @@ void wb_supervisor_field_set_mf_string(WbFieldRef field, int index, const char * union WbFieldData data; data.sf_string = supervisor_strdup(value); - field_operation_with_data(field, SET, index, data); + field_operation_with_data(field, SET, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_bool(WbFieldRef field, int index, bool value) { @@ -3403,7 +3404,7 @@ void wb_supervisor_field_insert_mf_bool(WbFieldRef field, int index, bool value) union WbFieldData data; data.sf_bool = value; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_int32(WbFieldRef field, int index, int value) { @@ -3412,7 +3413,7 @@ void wb_supervisor_field_insert_mf_int32(WbFieldRef field, int index, int value) union WbFieldData data; data.sf_int32 = value; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_float(WbFieldRef field, int index, double value) { @@ -3424,7 +3425,7 @@ void wb_supervisor_field_insert_mf_float(WbFieldRef field, int index, double val union WbFieldData data; data.sf_float = value; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_vec2f(WbFieldRef field, int index, const double values[2]) { @@ -3437,7 +3438,7 @@ void wb_supervisor_field_insert_mf_vec2f(WbFieldRef field, int index, const doub union WbFieldData data; data.sf_vec2f[0] = values[0]; data.sf_vec2f[1] = values[1]; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_vec3f(WbFieldRef field, int index, const double values[3]) { @@ -3451,7 +3452,7 @@ void wb_supervisor_field_insert_mf_vec3f(WbFieldRef field, int index, const doub data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_rotation(WbFieldRef field, int index, const double values[4]) { @@ -3471,7 +3472,7 @@ void wb_supervisor_field_insert_mf_rotation(WbFieldRef field, int index, const d data.sf_rotation[1] = values[1]; data.sf_rotation[2] = values[2]; data.sf_rotation[3] = values[3]; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_color(WbFieldRef field, int index, const double values[3]) { @@ -3492,7 +3493,7 @@ void wb_supervisor_field_insert_mf_color(WbFieldRef field, int index, const doub data.sf_vec3f[0] = values[0]; data.sf_vec3f[1] = values[1]; data.sf_vec3f[2] = values[2]; - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_insert_mf_string(WbFieldRef field, int index, const char *value) { @@ -3506,7 +3507,7 @@ void wb_supervisor_field_insert_mf_string(WbFieldRef field, int index, const cha union WbFieldData data; data.sf_string = supervisor_strdup(value); - field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data); + field_operation_with_data((WbFieldStruct *)field, IMPORT, index, data, __FUNCTION__); } void wb_supervisor_field_remove_mf(WbFieldRef field, int index) { @@ -3518,7 +3519,7 @@ void wb_supervisor_field_remove_mf(WbFieldRef field, int index) { if (!check_field(field, __FUNCTION__, WB_MF, false, &index, false, true)) return; - field_operation(field, REMOVE, index); + field_operation(field, REMOVE, index, __FUNCTION__); } void wb_supervisor_field_import_mf_node(WbFieldRef field, int position, const char *filename) { @@ -3577,7 +3578,7 @@ void wb_supervisor_field_import_mf_node(WbFieldRef field, int position, const ch union WbFieldData data; data.sf_string = supervisor_strdup(filename); create_and_append_field_request(f, IMPORT, position, data, false); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -3613,7 +3614,7 @@ void wb_supervisor_field_import_mf_node_from_string(WbFieldRef field, int positi union WbFieldData data; data.sf_string = supervisor_strdup(node_string); create_and_append_field_request(f, IMPORT_FROM_STRING, position, data, false); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -3630,7 +3631,7 @@ void wb_supervisor_field_remove_sf(WbFieldRef field) { if (!check_field(field, __FUNCTION__, WB_SF_NODE, true, NULL, false, true)) return; - field_operation(field, REMOVE, -1); + field_operation(field, REMOVE, -1, __FUNCTION__); field->count = 0; } @@ -3673,7 +3674,7 @@ void wb_supervisor_field_import_sf_node(WbFieldRef field, const char *filename) data.sf_string = supervisor_strdup(filename); create_and_append_field_request(f, IMPORT, -1, data, false); imported_node_id = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (imported_node_id >= 0) field->data.sf_node_uid = imported_node_id; robot_mutex_unlock_step(); @@ -3706,7 +3707,7 @@ void wb_supervisor_field_import_sf_node_from_string(WbFieldRef field, const char data.sf_string = supervisor_strdup(node_string); create_and_append_field_request(f, IMPORT_FROM_STRING, -1, data, false); imported_node_id = -1; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); if (imported_node_id >= 0) field->data.sf_node_uid = imported_node_id; robot_mutex_unlock_step(); diff --git a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c index 3883ae66b13..e163a2119d0 100644 --- a/tests/api/controllers/robot_parallel_step/robot_parallel_step.c +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -29,16 +29,19 @@ int main(int argc, char **argv) { wb_distance_sensor_enable(ds, time_step); wb_touch_sensor_enable(ts, time_step); + WbNodeRef robot_node = wb_supervisor_node_get_from_def("ROBOT"); + WbFieldRef translation_field = wb_supervisor_node_get_field(robot_node, "translation"); + WbFieldRef rotation_field = wb_supervisor_node_get_field(robot_node, "rotation"); + wb_supervisor_field_enable_sf_tracking(rotation_field, time_step); + // step 1 - 32 ms -> get initial sensor values, robot node/fields and update position wb_robot_step_begin(time_step); - WbNodeRef robot_node = wb_supervisor_node_get_from_def("ROBOT"); - WbFieldRef robot_trans_field = wb_supervisor_node_get_field(robot_node, "translation"); - WbFieldRef rotationField = wb_supervisor_node_get_field(robot_node, "rotation"); + const double *rotation = wb_supervisor_field_get_sf_rotation(rotation_field); const double newRotation[4] = {0.0, 1.0, 0.0, 1.5708}; const double newTranslation[3] = {-0.05, 0.0295, 0.1}; - wb_supervisor_field_set_sf_rotation(rotationField, newRotation); - wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation); + wb_supervisor_field_set_sf_rotation(rotation_field, newRotation); + wb_supervisor_field_set_sf_vec3f(translation_field, newTranslation); wb_robot_step_end(); @@ -72,8 +75,8 @@ int main(int argc, char **argv) { // second pose update const double newRotation1[4] = {0.0, 0.0, 1.0, 0.0}; const double newTranslation1[3] = {0.1, 0.0295, 0.1}; - wb_supervisor_field_set_sf_rotation(rotationField, newRotation1); - wb_supervisor_field_set_sf_vec3f(robot_trans_field, newTranslation1); + wb_supervisor_field_set_sf_rotation(rotation_field, newRotation1); + wb_supervisor_field_set_sf_vec3f(translation_field, newTranslation1); wb_robot_step_end(); // retrieve sensor values after step update From dcac09bcefe7f32708d6342554e5e7a8367005d1 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 12:25:37 +0100 Subject: [PATCH 28/39] flush_unlocked func is void again --- src/controller/c/robot.c | 8 +++----- src/controller/c/robot_private.h | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 1edf115fad6..27565080a05 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -946,14 +946,14 @@ WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, return robot.user_input_event_type; } -bool wb_robot_flush_unlocked(const char *func) { +void wb_robot_flush_unlocked(const char *func) { if (func && waiting_for_step_end) { fprintf( stderr, "Warning: %s(): functions with immediate requests to Webots cannot be implemented in-between wb_robot_step_begin() and " "wb_robot_step_end()!\n", func); - return 0; + return; } if (robot.webots_exit == WEBOTS_EXIT_NOW) { @@ -962,15 +962,13 @@ bool wb_robot_flush_unlocked(const char *func) { exit(EXIT_SUCCESS); } if (robot.webots_exit == WEBOTS_EXIT_LATER) - return 0; + return; robot.is_immediate_message = true; robot_send_request(0); robot_read_data(); if (robot.webots_exit == WEBOTS_EXIT_NOW) robot.webots_exit = WEBOTS_EXIT_LATER; robot.is_immediate_message = false; - - return 1; } int wb_robot_init_msvc() { diff --git a/src/controller/c/robot_private.h b/src/controller/c/robot_private.h index 3c60c982007..3b5671dc122 100644 --- a/src/controller/c/robot_private.h +++ b/src/controller/c/robot_private.h @@ -35,7 +35,7 @@ #endif int wb_robot_get_step_duration(); -bool wb_robot_flush_unlocked(); +void wb_robot_flush_unlocked(); void robot_write_request(WbDevice *, WbRequest *); void robot_read_answer(WbDevice *, WbRequest *); WbDevice *robot_get_device_with_node(WbDeviceTag tag, WbNodeType node, bool warning); From 77a6e672b170a30f5b03a681c3c7518f6d44baad Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 12:48:08 +0100 Subject: [PATCH 29/39] Update documentation for forbidden methods --- docs/reference/robot.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index 4f94f68ba07..d0eed4ffe03 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -260,6 +260,13 @@ If the two computational processes (Webots and controller) are slow, it may be i `wb_robot_step_begin` and `wb_robot_step_end` allow you to achieve such an implementation. They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. `wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. +Note that some functions cannot be implemented between `wb_robot_step_begin` and `wb_robot_step_end`, as they require immediate response from Webots using a request-response pattern. +This includes some [Supervisor API](#supervisor.md) functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. +Webots warns you in case you implement one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`. +You can simply write them before `wb_robot_step_begin` or after `wb_robot_step_end`. +If you still want to parallelize some of these functions with the simulation step, you can use the tracking options from the supervisor. +`wb_supervisor_field_enable_sf_tracking`, `wb_supervisor_node_enable_pose_tracking` and `wb_supervisor_node_enable_contact_point_tracking` force Webots to stream information to the controller. +Using tracking, some supervisor methods get their value locally and can be written between `wb_robot_step_begin` and `wb_robot_step_end`. The C API has two additional functions: `wb_robot_init` and `wb_robot_cleanup`. There is no equivalent of the `wb_robot_init` and `wb_robot_cleanup` functions in the Java, Python, C++ and MATLAB APIs. From 44178e0a7bad7ef34309fdeb5c9f00ef4c002769 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 12:51:07 +0100 Subject: [PATCH 30/39] Correct doc link --- docs/reference/robot.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index d0eed4ffe03..d53ed635052 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -261,7 +261,7 @@ If the two computational processes (Webots and controller) are slow, it may be i They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. `wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. Note that some functions cannot be implemented between `wb_robot_step_begin` and `wb_robot_step_end`, as they require immediate response from Webots using a request-response pattern. -This includes some [Supervisor API](#supervisor.md) functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. +This includes some [Supervisor API](supervisor.md)[ functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. Webots warns you in case you implement one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`. You can simply write them before `wb_robot_step_begin` or after `wb_robot_step_end`. If you still want to parallelize some of these functions with the simulation step, you can use the tracking options from the supervisor. From f2247efd49ed9a66df6bddbd705650a0813c883d Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 14:35:58 +0100 Subject: [PATCH 31/39] artifact in documentation --- docs/reference/robot.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index d53ed635052..c4de4cc0bb2 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -261,7 +261,7 @@ If the two computational processes (Webots and controller) are slow, it may be i They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. `wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. Note that some functions cannot be implemented between `wb_robot_step_begin` and `wb_robot_step_end`, as they require immediate response from Webots using a request-response pattern. -This includes some [Supervisor API](supervisor.md)[ functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. +This includes some [Supervisor API](supervisor.md) functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. Webots warns you in case you implement one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`. You can simply write them before `wb_robot_step_begin` or after `wb_robot_step_end`. If you still want to parallelize some of these functions with the simulation step, you can use the tracking options from the supervisor. From ba0b26af1b123983e21607f48167643c36c423d1 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 15:00:58 +0100 Subject: [PATCH 32/39] Update robot_private.h --- src/controller/c/robot_private.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/c/robot_private.h b/src/controller/c/robot_private.h index 3b5671dc122..f7e8151138c 100644 --- a/src/controller/c/robot_private.h +++ b/src/controller/c/robot_private.h @@ -35,7 +35,7 @@ #endif int wb_robot_get_step_duration(); -void wb_robot_flush_unlocked(); +void wb_robot_flush_unlocked(const char *); void robot_write_request(WbDevice *, WbRequest *); void robot_read_answer(WbDevice *, WbRequest *); WbDevice *robot_get_device_with_node(WbDeviceTag tag, WbNodeType node, bool warning); From b13b73024d455ad11b9a51ea0ca5694fcec49970 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 15:14:27 +0100 Subject: [PATCH 33/39] Updated flush_unlocked calls from other APIs --- src/controller/c/brake.c | 2 +- src/controller/c/display.c | 2 +- src/controller/c/motor.c | 2 +- src/controller/c/position_sensor.c | 2 +- src/controller/c/skin.c | 8 ++++---- tests/api/worlds/robot_parallel_step.wbt | 4 ++++ 6 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/controller/c/brake.c b/src/controller/c/brake.c index bff6ce45fbf..1246f94320f 100644 --- a/src/controller/c/brake.c +++ b/src/controller/c/brake.c @@ -148,7 +148,7 @@ static WbDeviceTag brake_get_associated_device(WbDeviceTag t, int device_type, c robot_mutex_lock_step(); b->state |= C_BRAKE_GET_ASSOCIATED_DEVICE; b->requested_device_type = device_type; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(function_name); WbDeviceTag result = b->requested_device_tag; robot_mutex_unlock_step(); return result; diff --git a/src/controller/c/display.c b/src/controller/c/display.c index 2523ab3f2c5..f75126041dc 100644 --- a/src/controller/c/display.c +++ b/src/controller/c/display.c @@ -855,7 +855,7 @@ void wb_display_image_save(WbDeviceTag tag, WbImageRef ir, const char *filename) message_enqueue(d, m); d->save_orders = o; } - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } diff --git a/src/controller/c/motor.c b/src/controller/c/motor.c index 3653e138a3b..0de81598077 100644 --- a/src/controller/c/motor.c +++ b/src/controller/c/motor.c @@ -655,7 +655,7 @@ static WbDeviceTag motor_get_associated_device(WbDeviceTag t, int device_type, c robot_mutex_lock_step(); motor->requests[C_MOTOR_GET_ASSOCIATED_DEVICE] = 1; motor->requested_device_type = device_type; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(function_name); WbDeviceTag tag = motor->associated_device_tag; robot_mutex_unlock_step(); return tag; diff --git a/src/controller/c/position_sensor.c b/src/controller/c/position_sensor.c index afea353f048..08191551601 100644 --- a/src/controller/c/position_sensor.c +++ b/src/controller/c/position_sensor.c @@ -186,7 +186,7 @@ static WbDeviceTag position_sensor_get_associated_device(WbDeviceTag tag, int de robot_mutex_lock_step(); p->requested_device_type = device_type; - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(function_name); WbDeviceTag result = p->requested_device_tag; robot_mutex_unlock_step(); return result; diff --git a/src/controller/c/skin.c b/src/controller/c/skin.c index d519855fcc0..76deb586363 100644 --- a/src/controller/c/skin.c +++ b/src/controller/c/skin.c @@ -186,7 +186,7 @@ void wb_skin_set_bone_orientation(WbDeviceTag tag, int index, const double orien add_request(skin, request); } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -219,7 +219,7 @@ void wb_skin_set_bone_position(WbDeviceTag tag, int index, const double position add_request(skin, request); } else fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); } @@ -274,7 +274,7 @@ const double *wb_skin_get_bone_position(WbDeviceTag tag, int index, bool absolut request->absolute = absolute; request->next = NULL; add_request(skin, request); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return skin->bone_position; @@ -301,7 +301,7 @@ const double *wb_skin_get_bone_orientation(WbDeviceTag tag, int index, bool abso request->absolute = absolute; request->next = NULL; add_request(skin, request); - wb_robot_flush_unlocked(); + wb_robot_flush_unlocked(__FUNCTION__); robot_mutex_unlock_step(); return skin->bone_orientation; diff --git a/tests/api/worlds/robot_parallel_step.wbt b/tests/api/worlds/robot_parallel_step.wbt index 2745a1c8736..28df83b8b6b 100644 --- a/tests/api/worlds/robot_parallel_step.wbt +++ b/tests/api/worlds/robot_parallel_step.wbt @@ -79,6 +79,8 @@ DEF ROBOT Robot { } type "force" } + TestSuiteEmitter { + } ] boundingObject Transform { translation 0 -0.004 0 @@ -95,3 +97,5 @@ DEF ROBOT Robot { controller "robot_parallel_step" supervisor TRUE } +TestSuiteSupervisor { +} From 0a5efb2aa1d34f94904cc41ab226dd18092868c7 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Thu, 13 Jan 2022 17:30:22 +0100 Subject: [PATCH 34/39] Update controller.i --- src/controller/python/controller.i | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/python/controller.i b/src/controller/python/controller.i index 8a4e6172bbd..d3463fb4dec 100644 --- a/src/controller/python/controller.i +++ b/src/controller/python/controller.i @@ -84,8 +84,8 @@ using namespace std; //---------------------------------------------------------------------------------------------- %thread webots::Robot::step(int duration); -%thread webots::Robot::step_begin(int duration); -%thread webots::Robot::step_end(); +%thread webots::Robot::stepBegin(int duration); +%thread webots::Robot::stepEnd(); %nothreadblock; //handling std::string From 0abd9b6657a0456f28e7a50da55a1a6782fac394 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 14 Jan 2022 09:47:10 +0100 Subject: [PATCH 35/39] Update documentation from code review Co-authored-by: Olivier Michel --- docs/reference/robot.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index c4de4cc0bb2..16fe8a980e9 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -260,13 +260,13 @@ If the two computational processes (Webots and controller) are slow, it may be i `wb_robot_step_begin` and `wb_robot_step_end` allow you to achieve such an implementation. They correspond to a split version of `wb_robot_step`, with the particularity that the code written between the two function calls is executed in parallel with the Webots simulation step. `wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. -Note that some functions cannot be implemented between `wb_robot_step_begin` and `wb_robot_step_end`, as they require immediate response from Webots using a request-response pattern. +Note that some Webots API functions cannot be called between `wb_robot_step_begin` and `wb_robot_step_end`, as they require immediate response from Webots using a request-response pattern. This includes some [Supervisor API](supervisor.md) functions, like `wb_supervisor_node_get_field` and `wb_supervisor_field_get_sf_rotation`, but also some Robot API functions, like `wb_robot_get_urdf`. -Webots warns you in case you implement one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`. -You can simply write them before `wb_robot_step_begin` or after `wb_robot_step_end`. -If you still want to parallelize some of these functions with the simulation step, you can use the tracking options from the supervisor. -`wb_supervisor_field_enable_sf_tracking`, `wb_supervisor_node_enable_pose_tracking` and `wb_supervisor_node_enable_contact_point_tracking` force Webots to stream information to the controller. -Using tracking, some supervisor methods get their value locally and can be written between `wb_robot_step_begin` and `wb_robot_step_end`. +Webots will warn you in case you call one of these functions between `wb_robot_step_begin` and `wb_robot_step_end`. +You can simply call them before `wb_robot_step_begin` or after `wb_robot_step_end`. +However, some of these functions can be called between `wb_robot_step_begin` and `wb_robot_step_end` if you enable the supervisor tracking feature. +`wb_supervisor_field_enable_sf_tracking`, `wb_supervisor_node_enable_pose_tracking` and `wb_supervisor_node_enable_contact_point_tracking` force Webots to continuously stream the requested information to the controller. +By enabling the tracking, the corresponding supervisor functions can be called between `wb_robot_step_begin` and `wb_robot_step_end`, because their value will be queried to Webots during `wb_robot_step_begin` and received during `wb_robot_step_end`. The C API has two additional functions: `wb_robot_init` and `wb_robot_cleanup`. There is no equivalent of the `wb_robot_init` and `wb_robot_cleanup` functions in the Java, Python, C++ and MATLAB APIs. From 5ac42457ecdb549c51a21cc1af40495fbc67a248 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Fri, 14 Jan 2022 09:58:20 +0100 Subject: [PATCH 36/39] func -> function symbol --- src/controller/c/robot.c | 6 +++--- src/controller/c/supervisor.c | 26 +++++++++++++------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/controller/c/robot.c b/src/controller/c/robot.c index 27565080a05..a386ab1dabc 100644 --- a/src/controller/c/robot.c +++ b/src/controller/c/robot.c @@ -946,13 +946,13 @@ WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, return robot.user_input_event_type; } -void wb_robot_flush_unlocked(const char *func) { - if (func && waiting_for_step_end) { +void wb_robot_flush_unlocked(const char *function) { + if (function && waiting_for_step_end) { fprintf( stderr, "Warning: %s(): functions with immediate requests to Webots cannot be implemented in-between wb_robot_step_begin() and " "wb_robot_step_end()!\n", - func); + function); return; } diff --git a/src/controller/c/supervisor.c b/src/controller/c/supervisor.c index b4105ef0b83..9c452412b05 100644 --- a/src/controller/c/supervisor.c +++ b/src/controller/c/supervisor.c @@ -1236,7 +1236,7 @@ static void create_and_append_field_request(WbFieldStruct *f, int action, int in is_field_immediate_message = request->type != SET; // set operations are postponed } -static void field_operation_with_data(WbFieldStruct *f, int action, int index, union WbFieldData data, const char *func) { +static void field_operation_with_data(WbFieldStruct *f, int action, int index, union WbFieldData data, const char *function) { robot_mutex_lock_step(); WbFieldRequest *r; for (r = field_requests_list_head; r; r = r->next) { @@ -1270,26 +1270,26 @@ static void field_operation_with_data(WbFieldStruct *f, int action, int index, u // pending get request should remain create_and_append_field_request(f, action, index, data, true); if (action != SET) // Only setter can be postponed. The getter, import and remove actions have to be applied immediately. - wb_robot_flush_unlocked(func); + wb_robot_flush_unlocked(function); assert(action != GET || sent_field_get_request == NULL); robot_mutex_unlock_step(); } -static void field_operation(WbFieldStruct *f, int action, int index, const char *func) { +static void field_operation(WbFieldStruct *f, int action, int index, const char *function) { union WbFieldData data; data.sf_string = NULL; - field_operation_with_data(f, action, index, data, func); + field_operation_with_data(f, action, index, data, function); } -static bool check_field(WbFieldRef f, const char *func, WbFieldType type, bool check_type, int *index, bool is_importing, +static bool check_field(WbFieldRef f, const char *function, WbFieldType type, bool check_type, int *index, bool is_importing, bool check_type_internal) { - if (!robot_check_supervisor(func)) + if (!robot_check_supervisor(function)) return false; if (!f) { if (!robot_is_quitting()) - fprintf(stderr, "Error: %s() called with NULL 'field' argument.\n", func); + fprintf(stderr, "Error: %s() called with NULL 'field' argument.\n", function); return false; } @@ -1304,18 +1304,18 @@ static bool check_field(WbFieldRef f, const char *func, WbFieldType type, bool c field = field->next; } if (!found) { - fprintf(stderr, "Error: %s() called with invalid 'field' argument.\n", func); + fprintf(stderr, "Error: %s() called with invalid 'field' argument.\n", function); return false; } if (check_type_internal && ((WbFieldStruct *)f)->is_proto_internal) { - fprintf(stderr, "Error: %s() called on a read-only PROTO internal field.\n", func); + fprintf(stderr, "Error: %s() called on a read-only PROTO internal field.\n", function); return false; } if (check_type && ((WbFieldStruct *)f)->type != type) { if (!robot_is_quitting()) - fprintf(stderr, "Error: %s() called with wrong field type: %s.\n", func, wb_supervisor_field_get_type_name(f)); + fprintf(stderr, "Error: %s() called with wrong field type: %s.\n", function, wb_supervisor_field_get_type_name(f)); return false; } @@ -1325,7 +1325,7 @@ static bool check_field(WbFieldRef f, const char *func, WbFieldType type, bool c int offset = is_importing ? 0 : -1; if (*index < -(count + 1 + offset) || *index > (count + offset)) { - fprintf(stderr, "Error: %s() called with an out-of-bound index: %d (should be between %d and %d).\n", func, *index, + fprintf(stderr, "Error: %s() called with an out-of-bound index: %d (should be between %d and %d).\n", function, *index, -count - 1 - offset, count + offset); return false; } @@ -1813,14 +1813,14 @@ int wb_supervisor_node_get_id(WbNodeRef node) { return node->id; } -static WbNodeRef node_get_from_id(int id, const char *func) { +static WbNodeRef node_get_from_id(int id, const char *function) { robot_mutex_lock_step(); WbNodeRef result = find_node_by_id(id); if (!result) { WbNodeRef node_list_before = node_list; node_id = id; - wb_robot_flush_unlocked(func); + wb_robot_flush_unlocked(function); if (node_list != node_list_before) result = node_list; else From 0723e6eb74611344b0774e3445da2cfa6ca7c410 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Fri, 14 Jan 2022 12:09:14 +0100 Subject: [PATCH 37/39] Remove virtual methods --- include/controller/cpp/webots/Robot.hpp | 4 ++-- lib/controller/matlab/wb_robot_step_begin.m | 6 ++++++ lib/controller/matlab/wb_robot_step_end.m | 6 ++++++ 3 files changed, 14 insertions(+), 2 deletions(-) create mode 100644 lib/controller/matlab/wb_robot_step_begin.m create mode 100644 lib/controller/matlab/wb_robot_step_end.m diff --git a/include/controller/cpp/webots/Robot.hpp b/include/controller/cpp/webots/Robot.hpp index e75b46405c2..6c49858aa60 100644 --- a/include/controller/cpp/webots/Robot.hpp +++ b/include/controller/cpp/webots/Robot.hpp @@ -68,8 +68,8 @@ namespace webots { virtual ~Robot(); virtual int step(int duration); - virtual int stepBegin(int duration); - virtual int stepEnd(); + int stepBegin(int duration); + int stepEnd(); UserInputEvent waitForUserInputEvent(UserInputEvent event_type, int timeout); std::string getName() const; std::string getUrdf(std::string prefix = "") const; diff --git a/lib/controller/matlab/wb_robot_step_begin.m b/lib/controller/matlab/wb_robot_step_begin.m new file mode 100644 index 00000000000..a4a7806fc52 --- /dev/null +++ b/lib/controller/matlab/wb_robot_step_begin.m @@ -0,0 +1,6 @@ +function result = wb_robot_step_begin(duration) +% Usage: wb_robot_step_begin(duration) +% Matlab API for Webots +% Online documentation is available here + +result = calllib('libController', 'wb_robot_step_begin', duration); diff --git a/lib/controller/matlab/wb_robot_step_end.m b/lib/controller/matlab/wb_robot_step_end.m new file mode 100644 index 00000000000..79b2edf840f --- /dev/null +++ b/lib/controller/matlab/wb_robot_step_end.m @@ -0,0 +1,6 @@ +function result = wb_robot_step_end() +% Usage: wb_robot_step_end() +% Matlab API for Webots +% Online documentation is available here + +result = calllib('libController', 'wb_robot_step_end'); From 52595483a5abf06ca604f264ccfcba994e410cd8 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz Date: Fri, 14 Jan 2022 12:27:12 +0100 Subject: [PATCH 38/39] remove matlab files --- lib/controller/matlab/wb_robot_step_begin.m | 6 ------ lib/controller/matlab/wb_robot_step_end.m | 6 ------ 2 files changed, 12 deletions(-) delete mode 100644 lib/controller/matlab/wb_robot_step_begin.m delete mode 100644 lib/controller/matlab/wb_robot_step_end.m diff --git a/lib/controller/matlab/wb_robot_step_begin.m b/lib/controller/matlab/wb_robot_step_begin.m deleted file mode 100644 index a4a7806fc52..00000000000 --- a/lib/controller/matlab/wb_robot_step_begin.m +++ /dev/null @@ -1,6 +0,0 @@ -function result = wb_robot_step_begin(duration) -% Usage: wb_robot_step_begin(duration) -% Matlab API for Webots -% Online documentation is available here - -result = calllib('libController', 'wb_robot_step_begin', duration); diff --git a/lib/controller/matlab/wb_robot_step_end.m b/lib/controller/matlab/wb_robot_step_end.m deleted file mode 100644 index 79b2edf840f..00000000000 --- a/lib/controller/matlab/wb_robot_step_end.m +++ /dev/null @@ -1,6 +0,0 @@ -function result = wb_robot_step_end() -% Usage: wb_robot_step_end() -% Matlab API for Webots -% Online documentation is available here - -result = calllib('libController', 'wb_robot_step_end'); From 0d546d9a7aec7abc2e06509195994f00dc81be02 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 14 Jan 2022 15:35:20 +0100 Subject: [PATCH 39/39] Remove virtuals from doc Co-authored-by: Olivier Michel --- docs/reference/robot.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/reference/robot.md b/docs/reference/robot.md index 16fe8a980e9..d775626cc31 100644 --- a/docs/reference/robot.md +++ b/docs/reference/robot.md @@ -159,8 +159,8 @@ namespace webots { Robot(); virtual ~Robot(); virtual int step(int duration); - virtual int stepBegin(int duration); - virtual int stepEnd(); + int stepBegin(int duration); + int stepEnd(); // ... } }