diff --git a/docs/reference/robot.md b/docs/reference/robot.md index a77d6d48a89..d775626cc31 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); + int stepBegin(int duration); + int stepEnd(); // ... } } @@ -171,6 +177,8 @@ class Robot: def __init__(self): def __del__(self): def step(self, duration): + def stepBegin(self, duration): + def stepEnd(self): # ... ``` @@ -185,6 +193,8 @@ public class Robot { public Robot(); protected void finalize(); public int step(int duration); + public int stepBegin(int duration); + public int stepEnd(); // ... } ``` @@ -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,22 @@ 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. +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. +`wb_robot_step_begin` and `wb_robot_step_end` both return -1 if the simulation is terminated. +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 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. In these languages the necessary initialization and cleanup of the controller library is done automatically. @@ -256,7 +284,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" @@ -443,6 +471,275 @@ end %tab-end +%end + +**Parallel 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 Webots simulation step */ + + /* 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); + + /* 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 (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 + + 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 (stepEnd() != -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.stepBegin(self.timeStep) == -1: + 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 + + 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 + +# 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 (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 + + 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 (stepEnd() != -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 wb_robot_step_end) is executed in parallel with the background simulation step + + 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 + % leave the loop when the simulation is over + if wb_robot_step_end() == -1 + break; + end +end +``` + +%tab-end + + %end --- diff --git a/include/controller/c/webots/robot.h b/include/controller/c/webots/robot.h index ff2d974ef17..574e767d67d 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); // 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 diff --git a/include/controller/cpp/webots/Robot.hpp b/include/controller/cpp/webots/Robot.hpp index eea84592c92..6c49858aa60 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); + 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/.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/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..87c8b8089d8 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/.gitignore @@ -0,0 +1,2 @@ +/parallel_robot_step +/*.png 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..ec58d02f907 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/controllers/parallel_robot_step/parallel_robot_step.c @@ -0,0 +1,170 @@ +/* + * 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. + */ + +/* + * 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 + +#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; + 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(); + + 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"); + 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); + + /* 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 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) { + left_speed = 0; + right_speed = 0; + } else { + /* 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 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 */ + } 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..77d7e0d4fe0 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/worlds/.parallel_robot_step.wbproj @@ -0,0 +1,9 @@ +Webots Project File version R2022b +perspectives: 000000ff00000000fd00000003000000000000006900000388fc0100000002fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000000000000000fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007700000000000000006900000000000000000000000100000287000002acfc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002ac0000008900ffffff0000000300000780000000dafc0100000002fb0000000e0043006f006e0073006f006c006501000000000000073f0000000000000000fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c0100000000000007800000006900ffffff000004f7000002ac00000001000000020000000100000008fc00000000 +simulationViewPerspectives: 000000ff00000001000000020000016c000004a00100000002010000000101 +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..a8a112c7867 --- /dev/null +++ b/projects/samples/howto/parallel_robot_step/worlds/parallel_robot_step.wbt @@ -0,0 +1,306 @@ +#VRML_SIM R2022b utf8 +WorldInfo { + info [ + "Example use of a color Camera device." + ] + title "Camera" +} +Viewpoint { + orientation -0.17458317664901404 -0.7286632169171916 0.6622466540066693 5.6399536822144265 + position -1.298765515155383 0.6306794765789305 0.904897417937484 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +RectangleArena { +} +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" +} 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 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/robot.c b/src/controller/c/robot.c index 9d8fa177b84..a386ab1dabc 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() { @@ -554,21 +556,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 +797,10 @@ 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 (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); @@ -846,8 +836,33 @@ 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); + + 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) + 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); @@ -855,6 +870,21 @@ int wb_robot_step(int duration) { robot_mutex_unlock_step(); robot_window_read_sensors(); + waiting_for_step_begin = true; + waiting_for_step_end = false; + + return e; +} + +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; + e = wb_robot_step_end(); + return e; } @@ -896,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(); @@ -916,7 +946,16 @@ WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, return robot.user_input_event_type; } -void wb_robot_flush_unlocked() { +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", + function); + return; + } + if (robot.webots_exit == WEBOTS_EXIT_NOW) { robot_quit(); robot_mutex_unlock_step(); @@ -1136,7 +1175,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(); } @@ -1169,7 +1208,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..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); 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/src/controller/c/supervisor.c b/src/controller/c/supervisor.c index 2345b9d32d0..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) { +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,25 +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(); + 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) { +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); + 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; } @@ -1303,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; } @@ -1324,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; } @@ -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 *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(); + wb_robot_flush_unlocked(function); 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/src/controller/cpp/Robot.cpp b/src/controller/cpp/Robot.cpp index bd7f1584bc0..861da620004 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::stepBegin(int duration) { + return wb_robot_step_begin(duration); +} + +int Robot::stepEnd() { + 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)); } 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..d3463fb4dec 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::stepBegin(int duration); +%thread webots::Robot::stepEnd(); %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 " 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_parallel_step/Makefile b/tests/api/controllers/robot_parallel_step/Makefile new file mode 100644 index 00000000000..21c889fdcf7 --- /dev/null +++ b/tests/api/controllers/robot_parallel_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/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..e163a2119d0 --- /dev/null +++ b/tests/api/controllers/robot_parallel_step/robot_parallel_step.c @@ -0,0 +1,110 @@ +#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]); + + 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"); + 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); + + 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); + + 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(rotation_field, newRotation); + wb_supervisor_field_set_sf_vec3f(translation_field, newTranslation); + + wb_robot_step_end(); + + // step 2 - 64 ms -> check initial sensor values + wb_robot_step_begin(time_step); + + // 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 + 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(rotation_field, newRotation1); + wb_supervisor_field_set_sf_vec3f(translation_field, newTranslation1); + + wb_robot_step_end(); // retrieve sensor values after step 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 < 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); + 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_parallel_step.wbt b/tests/api/worlds/robot_parallel_step.wbt new file mode 100644 index 00000000000..28df83b8b6b --- /dev/null +++ b/tests/api/worlds/robot_parallel_step.wbt @@ -0,0 +1,101 @@ +#VRML_SIM R2022b utf8 +WorldInfo { + coordinateSystem "NUE" +} +Viewpoint { + orientation 0.9286315153002099 -0.37098708232974925 0.00347757612369663 4.528139423982821 + position -0.4333492851082345 0.279699359957294 0.3018558005363777 +} +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 +} +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 { + rotation 0 1 0 1.57079632679 + children [ + Shape { + geometry Cone { + bottomRadius 0.005 + height 0.02 + } + } + ] + } + Camera { + rotation 1 1.88116e-06 -7.19235e-09 -1.5708053071795867 + 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_parallel_step" + supervisor TRUE +} +TestSuiteSupervisor { +}