From c4e52d7e5241c599f96fdb9b27fde90c65f6402d Mon Sep 17 00:00:00 2001 From: insleker Date: Wed, 20 May 2026 19:11:32 +0800 Subject: [PATCH] feat: copy webots controller lib sources from webots upstream --- CMakeLists.txt | 0 conanfile.py | 0 include/controller/c/webots/accelerometer.h | 45 + include/controller/c/webots/altimeter.h | 41 + include/controller/c/webots/brake.h | 44 + include/controller/c/webots/camera.h | 104 + .../c/webots/camera_recognition_object.h | 36 + include/controller/c/webots/compass.h | 44 + include/controller/c/webots/connector.h | 43 + include/controller/c/webots/console.h | 36 + include/controller/c/webots/contact_point.h | 26 + include/controller/c/webots/device.h | 43 + include/controller/c/webots/display.h | 72 + include/controller/c/webots/distance_sensor.h | 56 + include/controller/c/webots/emitter.h | 46 + include/controller/c/webots/gps.h | 49 + include/controller/c/webots/gyro.h | 44 + include/controller/c/webots/inertial_unit.h | 44 + include/controller/c/webots/joystick.h | 57 + include/controller/c/webots/keyboard.h | 61 + include/controller/c/webots/led.h | 38 + include/controller/c/webots/lidar.h | 66 + include/controller/c/webots/lidar_point.h | 32 + include/controller/c/webots/light_sensor.h | 44 + include/controller/c/webots/microphone.h | 44 + include/controller/c/webots/motor.h | 75 + include/controller/c/webots/mouse.h | 50 + include/controller/c/webots/mouse_state.h | 40 + include/controller/c/webots/nodes.h | 122 + include/controller/c/webots/pen.h | 38 + .../c/webots/plugins/robot_window/default.h | 41 + .../generic_robot_window/generic.h | 44 + .../plugins/robot_window/robot_window.h | 30 + .../c/webots/plugins/robot_window/robot_wwi.h | 36 + include/controller/c/webots/position_sensor.h | 47 + include/controller/c/webots/radar.h | 53 + include/controller/c/webots/radar_target.h | 31 + include/controller/c/webots/radio.h | 75 + include/controller/c/webots/range_finder.h | 50 + include/controller/c/webots/receiver.h | 53 + include/controller/c/webots/remote_control.h | 128 + include/controller/c/webots/robot.h | 133 + include/controller/c/webots/skin.h | 42 + include/controller/c/webots/speaker.h | 49 + include/controller/c/webots/supervisor.h | 252 + include/controller/c/webots/touch_sensor.h | 50 + include/controller/c/webots/types.h | 83 + .../controller/c/webots/utils/ansi_codes.h | 63 + include/controller/c/webots/utils/motion.h | 47 + include/controller/c/webots/utils/string.h | 35 + include/controller/c/webots/utils/system.h | 55 + include/controller/c/webots/vacuum_gripper.h | 43 + include/controller/c/webots/vehicle/car.h | 87 + include/controller/c/webots/vehicle/driver.h | 97 + .../controller/cpp/webots/Accelerometer.hpp | 35 + include/controller/cpp/webots/Altimeter.hpp | 33 + include/controller/cpp/webots/Brake.hpp | 55 + include/controller/cpp/webots/Camera.hpp | 68 + include/controller/cpp/webots/Compass.hpp | 35 + include/controller/cpp/webots/Connector.hpp | 36 + include/controller/cpp/webots/Device.hpp | 43 + include/controller/cpp/webots/Display.hpp | 54 + .../controller/cpp/webots/DistanceSensor.hpp | 41 + include/controller/cpp/webots/Emitter.hpp | 36 + include/controller/cpp/webots/Field.hpp | 128 + include/controller/cpp/webots/GPS.hpp | 43 + include/controller/cpp/webots/Gyro.hpp | 35 + include/controller/cpp/webots/ImageRef.hpp | 34 + .../controller/cpp/webots/InertialUnit.hpp | 35 + include/controller/cpp/webots/Joystick.hpp | 43 + include/controller/cpp/webots/Keyboard.hpp | 51 + include/controller/cpp/webots/LED.hpp | 31 + include/controller/cpp/webots/Lidar.hpp | 52 + include/controller/cpp/webots/LightSensor.hpp | 35 + include/controller/cpp/webots/Motor.hpp | 84 + include/controller/cpp/webots/Mouse.hpp | 37 + include/controller/cpp/webots/Node.hpp | 193 + include/controller/cpp/webots/Pen.hpp | 31 + .../controller/cpp/webots/PositionSensor.hpp | 57 + include/controller/cpp/webots/Proto.hpp | 46 + include/controller/cpp/webots/Radar.hpp | 41 + include/controller/cpp/webots/RangeFinder.hpp | 39 + include/controller/cpp/webots/Receiver.hpp | 41 + include/controller/cpp/webots/Robot.hpp | 181 + include/controller/cpp/webots/Skin.hpp | 35 + include/controller/cpp/webots/Speaker.hpp | 39 + include/controller/cpp/webots/Supervisor.hpp | 90 + include/controller/cpp/webots/TouchSensor.hpp | 43 + .../controller/cpp/webots/VacuumGripper.hpp | 36 + .../controller/cpp/webots/utils/AnsiCodes.hpp | 47 + .../controller/cpp/webots/utils/Motion.hpp | 45 + include/controller/cpp/webots/vehicle/Car.hpp | 65 + .../controller/cpp/webots/vehicle/Driver.hpp | 92 + src/controller/c/Controller.def | 643 +++ src/controller/c/README.TXT | 5 + src/controller/c/abstract_camera.c | 171 + src/controller/c/abstract_camera.h | 64 + src/controller/c/accelerometer.c | 191 + src/controller/c/altimeter.c | 144 + src/controller/c/ansi_codes.c | 28 + src/controller/c/base64.c | 59 + src/controller/c/base64.h | 22 + src/controller/c/brake.c | 164 + src/controller/c/camera.c | 894 ++++ src/controller/c/camera_private.h | 48 + src/controller/c/compass.c | 190 + src/controller/c/connector.c | 187 + src/controller/c/console.c | 28 + src/controller/c/default_robot_window.c | 895 ++++ .../c/default_robot_window_private.h | 22 + src/controller/c/device.c | 171 + src/controller/c/device_private.h | 43 + src/controller/c/display.c | 887 ++++ src/controller/c/display_private.h | 22 + src/controller/c/distance_sensor.c | 247 + src/controller/c/dynamic_library.c | 86 + src/controller/c/dynamic_library.h | 37 + src/controller/c/emitter.c | 340 ++ src/controller/c/file.c | 27 + src/controller/c/file.h | 25 + src/controller/c/g_image.c | 333 ++ src/controller/c/g_image.h | 62 + src/controller/c/g_pipe.c | 148 + src/controller/c/g_pipe.h | 55 + src/controller/c/gps.c | 231 + src/controller/c/gyro.c | 191 + src/controller/c/html_robot_window.c | 112 + src/controller/c/html_robot_window_private.h | 25 + src/controller/c/image.c | 74 + src/controller/c/image_private.h | 39 + src/controller/c/inertial_unit.c | 195 + src/controller/c/joystick.c | 352 ++ src/controller/c/joystick_private.h | 28 + src/controller/c/keyboard.c | 128 + src/controller/c/keyboard_private.h | 28 + src/controller/c/led.c | 102 + src/controller/c/lidar.c | 516 +++ src/controller/c/light_sensor.c | 186 + src/controller/c/messages.h | 310 ++ src/controller/c/microphone.c | 181 + src/controller/c/motion.c | 564 +++ src/controller/c/motion_private.h | 23 + src/controller/c/motor.c | 673 +++ src/controller/c/mouse.c | 135 + src/controller/c/mouse_private.h | 27 + src/controller/c/node.c | 241 + src/controller/c/pen.c | 120 + src/controller/c/percent.c | 53 + src/controller/c/percent.h | 22 + src/controller/c/position_sensor.c | 202 + src/controller/c/radar.c | 253 + src/controller/c/radio.c | 476 ++ src/controller/c/range_finder.c | 335 ++ src/controller/c/receiver.c | 381 ++ src/controller/c/remote_control.c | 490 ++ src/controller/c/remote_control_private.h | 41 + src/controller/c/request.c | 223 + src/controller/c/request.h | 74 + src/controller/c/robot.c | 1659 +++++++ src/controller/c/robot_private.h | 60 + src/controller/c/robot_window.c | 166 + src/controller/c/robot_window_private.h | 32 + src/controller/c/scheduler.c | 370 ++ src/controller/c/scheduler.h | 45 + src/controller/c/sha1.c | 252 + src/controller/c/sha1.h | 52 + src/controller/c/skin.c | 308 ++ src/controller/c/speaker.c | 616 +++ src/controller/c/string.c | 80 + src/controller/c/supervisor.c | 4074 +++++++++++++++++ src/controller/c/supervisor_private.h | 24 + src/controller/c/system.c | 132 + src/controller/c/tcp_client.c | 116 + src/controller/c/tcp_client.h | 43 + src/controller/c/touch_sensor.c | 251 + src/controller/c/vacuum_gripper.c | 188 + src/controller/cpp/.gitignore | 2 + src/controller/cpp/Accelerometer.cpp | 43 + src/controller/cpp/Altimeter.cpp | 38 + src/controller/cpp/Brake.cpp | 51 + src/controller/cpp/Camera.cpp | 164 + src/controller/cpp/Compass.cpp | 43 + src/controller/cpp/Connector.cpp | 47 + src/controller/cpp/Device.cpp | 45 + src/controller/cpp/Display.cpp | 115 + src/controller/cpp/DistanceSensor.cpp | 59 + src/controller/cpp/Emitter.cpp | 43 + src/controller/cpp/Field.cpp | 264 ++ src/controller/cpp/GPS.cpp | 57 + src/controller/cpp/Gyro.cpp | 43 + src/controller/cpp/InertialUnit.cpp | 43 + src/controller/cpp/Joystick.cpp | 80 + src/controller/cpp/Keyboard.cpp | 35 + src/controller/cpp/LED.cpp | 27 + src/controller/cpp/Lidar.cpp | 104 + src/controller/cpp/LightSensor.cpp | 43 + src/controller/cpp/Motion.cpp | 64 + src/controller/cpp/Motor.cpp | 156 + src/controller/cpp/Mouse.cpp | 47 + src/controller/cpp/Node.cpp | 237 + src/controller/cpp/Pen.cpp | 27 + src/controller/cpp/PositionSensor.cpp | 63 + src/controller/cpp/Proto.cpp | 73 + src/controller/cpp/Radar.cpp | 55 + src/controller/cpp/RangeFinder.cpp | 64 + src/controller/cpp/Receiver.cpp | 63 + src/controller/cpp/Robot.cpp | 622 +++ src/controller/cpp/Skin.cpp | 46 + src/controller/cpp/Speaker.cpp | 56 + src/controller/cpp/Supervisor.cpp | 199 + src/controller/cpp/TouchSensor.cpp | 51 + src/controller/cpp/VacuumGripper.cpp | 47 + test_package/CMakeLists.txt | 0 test_package/conanfile.py | 0 214 files changed, 30182 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 conanfile.py create mode 100644 include/controller/c/webots/accelerometer.h create mode 100644 include/controller/c/webots/altimeter.h create mode 100644 include/controller/c/webots/brake.h create mode 100644 include/controller/c/webots/camera.h create mode 100644 include/controller/c/webots/camera_recognition_object.h create mode 100644 include/controller/c/webots/compass.h create mode 100644 include/controller/c/webots/connector.h create mode 100644 include/controller/c/webots/console.h create mode 100644 include/controller/c/webots/contact_point.h create mode 100644 include/controller/c/webots/device.h create mode 100644 include/controller/c/webots/display.h create mode 100644 include/controller/c/webots/distance_sensor.h create mode 100644 include/controller/c/webots/emitter.h create mode 100644 include/controller/c/webots/gps.h create mode 100644 include/controller/c/webots/gyro.h create mode 100644 include/controller/c/webots/inertial_unit.h create mode 100644 include/controller/c/webots/joystick.h create mode 100644 include/controller/c/webots/keyboard.h create mode 100644 include/controller/c/webots/led.h create mode 100644 include/controller/c/webots/lidar.h create mode 100644 include/controller/c/webots/lidar_point.h create mode 100644 include/controller/c/webots/light_sensor.h create mode 100644 include/controller/c/webots/microphone.h create mode 100644 include/controller/c/webots/motor.h create mode 100644 include/controller/c/webots/mouse.h create mode 100644 include/controller/c/webots/mouse_state.h create mode 100644 include/controller/c/webots/nodes.h create mode 100644 include/controller/c/webots/pen.h create mode 100644 include/controller/c/webots/plugins/robot_window/default.h create mode 100644 include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h create mode 100644 include/controller/c/webots/plugins/robot_window/robot_window.h create mode 100644 include/controller/c/webots/plugins/robot_window/robot_wwi.h create mode 100644 include/controller/c/webots/position_sensor.h create mode 100644 include/controller/c/webots/radar.h create mode 100644 include/controller/c/webots/radar_target.h create mode 100644 include/controller/c/webots/radio.h create mode 100644 include/controller/c/webots/range_finder.h create mode 100644 include/controller/c/webots/receiver.h create mode 100644 include/controller/c/webots/remote_control.h create mode 100644 include/controller/c/webots/robot.h create mode 100644 include/controller/c/webots/skin.h create mode 100644 include/controller/c/webots/speaker.h create mode 100644 include/controller/c/webots/supervisor.h create mode 100644 include/controller/c/webots/touch_sensor.h create mode 100644 include/controller/c/webots/types.h create mode 100644 include/controller/c/webots/utils/ansi_codes.h create mode 100644 include/controller/c/webots/utils/motion.h create mode 100644 include/controller/c/webots/utils/string.h create mode 100644 include/controller/c/webots/utils/system.h create mode 100644 include/controller/c/webots/vacuum_gripper.h create mode 100644 include/controller/c/webots/vehicle/car.h create mode 100644 include/controller/c/webots/vehicle/driver.h create mode 100644 include/controller/cpp/webots/Accelerometer.hpp create mode 100644 include/controller/cpp/webots/Altimeter.hpp create mode 100644 include/controller/cpp/webots/Brake.hpp create mode 100644 include/controller/cpp/webots/Camera.hpp create mode 100644 include/controller/cpp/webots/Compass.hpp create mode 100644 include/controller/cpp/webots/Connector.hpp create mode 100644 include/controller/cpp/webots/Device.hpp create mode 100644 include/controller/cpp/webots/Display.hpp create mode 100644 include/controller/cpp/webots/DistanceSensor.hpp create mode 100644 include/controller/cpp/webots/Emitter.hpp create mode 100644 include/controller/cpp/webots/Field.hpp create mode 100644 include/controller/cpp/webots/GPS.hpp create mode 100644 include/controller/cpp/webots/Gyro.hpp create mode 100644 include/controller/cpp/webots/ImageRef.hpp create mode 100644 include/controller/cpp/webots/InertialUnit.hpp create mode 100644 include/controller/cpp/webots/Joystick.hpp create mode 100644 include/controller/cpp/webots/Keyboard.hpp create mode 100644 include/controller/cpp/webots/LED.hpp create mode 100644 include/controller/cpp/webots/Lidar.hpp create mode 100644 include/controller/cpp/webots/LightSensor.hpp create mode 100644 include/controller/cpp/webots/Motor.hpp create mode 100644 include/controller/cpp/webots/Mouse.hpp create mode 100644 include/controller/cpp/webots/Node.hpp create mode 100644 include/controller/cpp/webots/Pen.hpp create mode 100644 include/controller/cpp/webots/PositionSensor.hpp create mode 100644 include/controller/cpp/webots/Proto.hpp create mode 100644 include/controller/cpp/webots/Radar.hpp create mode 100644 include/controller/cpp/webots/RangeFinder.hpp create mode 100644 include/controller/cpp/webots/Receiver.hpp create mode 100644 include/controller/cpp/webots/Robot.hpp create mode 100644 include/controller/cpp/webots/Skin.hpp create mode 100644 include/controller/cpp/webots/Speaker.hpp create mode 100644 include/controller/cpp/webots/Supervisor.hpp create mode 100644 include/controller/cpp/webots/TouchSensor.hpp create mode 100644 include/controller/cpp/webots/VacuumGripper.hpp create mode 100644 include/controller/cpp/webots/utils/AnsiCodes.hpp create mode 100644 include/controller/cpp/webots/utils/Motion.hpp create mode 100644 include/controller/cpp/webots/vehicle/Car.hpp create mode 100644 include/controller/cpp/webots/vehicle/Driver.hpp create mode 100644 src/controller/c/Controller.def create mode 100644 src/controller/c/README.TXT create mode 100644 src/controller/c/abstract_camera.c create mode 100644 src/controller/c/abstract_camera.h create mode 100644 src/controller/c/accelerometer.c create mode 100644 src/controller/c/altimeter.c create mode 100644 src/controller/c/ansi_codes.c create mode 100644 src/controller/c/base64.c create mode 100644 src/controller/c/base64.h create mode 100644 src/controller/c/brake.c create mode 100644 src/controller/c/camera.c create mode 100644 src/controller/c/camera_private.h create mode 100644 src/controller/c/compass.c create mode 100644 src/controller/c/connector.c create mode 100644 src/controller/c/console.c create mode 100644 src/controller/c/default_robot_window.c create mode 100644 src/controller/c/default_robot_window_private.h create mode 100644 src/controller/c/device.c create mode 100644 src/controller/c/device_private.h create mode 100644 src/controller/c/display.c create mode 100644 src/controller/c/display_private.h create mode 100644 src/controller/c/distance_sensor.c create mode 100644 src/controller/c/dynamic_library.c create mode 100644 src/controller/c/dynamic_library.h create mode 100644 src/controller/c/emitter.c create mode 100644 src/controller/c/file.c create mode 100644 src/controller/c/file.h create mode 100644 src/controller/c/g_image.c create mode 100644 src/controller/c/g_image.h create mode 100644 src/controller/c/g_pipe.c create mode 100644 src/controller/c/g_pipe.h create mode 100644 src/controller/c/gps.c create mode 100644 src/controller/c/gyro.c create mode 100644 src/controller/c/html_robot_window.c create mode 100644 src/controller/c/html_robot_window_private.h create mode 100644 src/controller/c/image.c create mode 100644 src/controller/c/image_private.h create mode 100644 src/controller/c/inertial_unit.c create mode 100644 src/controller/c/joystick.c create mode 100644 src/controller/c/joystick_private.h create mode 100644 src/controller/c/keyboard.c create mode 100644 src/controller/c/keyboard_private.h create mode 100644 src/controller/c/led.c create mode 100644 src/controller/c/lidar.c create mode 100644 src/controller/c/light_sensor.c create mode 100644 src/controller/c/messages.h create mode 100644 src/controller/c/microphone.c create mode 100644 src/controller/c/motion.c create mode 100644 src/controller/c/motion_private.h create mode 100644 src/controller/c/motor.c create mode 100644 src/controller/c/mouse.c create mode 100644 src/controller/c/mouse_private.h create mode 100644 src/controller/c/node.c create mode 100644 src/controller/c/pen.c create mode 100644 src/controller/c/percent.c create mode 100644 src/controller/c/percent.h create mode 100644 src/controller/c/position_sensor.c create mode 100644 src/controller/c/radar.c create mode 100644 src/controller/c/radio.c create mode 100644 src/controller/c/range_finder.c create mode 100644 src/controller/c/receiver.c create mode 100644 src/controller/c/remote_control.c create mode 100644 src/controller/c/remote_control_private.h create mode 100644 src/controller/c/request.c create mode 100644 src/controller/c/request.h create mode 100644 src/controller/c/robot.c create mode 100644 src/controller/c/robot_private.h create mode 100644 src/controller/c/robot_window.c create mode 100644 src/controller/c/robot_window_private.h create mode 100644 src/controller/c/scheduler.c create mode 100644 src/controller/c/scheduler.h create mode 100644 src/controller/c/sha1.c create mode 100644 src/controller/c/sha1.h create mode 100644 src/controller/c/skin.c create mode 100644 src/controller/c/speaker.c create mode 100644 src/controller/c/string.c create mode 100644 src/controller/c/supervisor.c create mode 100644 src/controller/c/supervisor_private.h create mode 100644 src/controller/c/system.c create mode 100644 src/controller/c/tcp_client.c create mode 100644 src/controller/c/tcp_client.h create mode 100644 src/controller/c/touch_sensor.c create mode 100644 src/controller/c/vacuum_gripper.c create mode 100644 src/controller/cpp/.gitignore create mode 100644 src/controller/cpp/Accelerometer.cpp create mode 100644 src/controller/cpp/Altimeter.cpp create mode 100644 src/controller/cpp/Brake.cpp create mode 100644 src/controller/cpp/Camera.cpp create mode 100644 src/controller/cpp/Compass.cpp create mode 100644 src/controller/cpp/Connector.cpp create mode 100644 src/controller/cpp/Device.cpp create mode 100644 src/controller/cpp/Display.cpp create mode 100644 src/controller/cpp/DistanceSensor.cpp create mode 100644 src/controller/cpp/Emitter.cpp create mode 100644 src/controller/cpp/Field.cpp create mode 100644 src/controller/cpp/GPS.cpp create mode 100644 src/controller/cpp/Gyro.cpp create mode 100644 src/controller/cpp/InertialUnit.cpp create mode 100644 src/controller/cpp/Joystick.cpp create mode 100644 src/controller/cpp/Keyboard.cpp create mode 100644 src/controller/cpp/LED.cpp create mode 100644 src/controller/cpp/Lidar.cpp create mode 100644 src/controller/cpp/LightSensor.cpp create mode 100644 src/controller/cpp/Motion.cpp create mode 100644 src/controller/cpp/Motor.cpp create mode 100644 src/controller/cpp/Mouse.cpp create mode 100644 src/controller/cpp/Node.cpp create mode 100644 src/controller/cpp/Pen.cpp create mode 100644 src/controller/cpp/PositionSensor.cpp create mode 100644 src/controller/cpp/Proto.cpp create mode 100644 src/controller/cpp/Radar.cpp create mode 100644 src/controller/cpp/RangeFinder.cpp create mode 100644 src/controller/cpp/Receiver.cpp create mode 100644 src/controller/cpp/Robot.cpp create mode 100644 src/controller/cpp/Skin.cpp create mode 100644 src/controller/cpp/Speaker.cpp create mode 100644 src/controller/cpp/Supervisor.cpp create mode 100644 src/controller/cpp/TouchSensor.cpp create mode 100644 src/controller/cpp/VacuumGripper.cpp create mode 100644 test_package/CMakeLists.txt create mode 100644 test_package/conanfile.py diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..e69de29 diff --git a/conanfile.py b/conanfile.py new file mode 100644 index 0000000..e69de29 diff --git a/include/controller/c/webots/accelerometer.h b/include/controller/c/webots/accelerometer.h new file mode 100644 index 0000000..a5447bc --- /dev/null +++ b/include/controller/c/webots/accelerometer.h @@ -0,0 +1,45 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Accelerometer node */ +/**********************************************************************************/ + +#ifndef WB_ACCELEROMETER_H +#define WB_ACCELEROMETER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_accelerometer_enable(WbDeviceTag tag, int sampling_period); +void wb_accelerometer_disable(WbDeviceTag tag); +int wb_accelerometer_get_sampling_period(WbDeviceTag tag); + +int wb_accelerometer_get_lookup_table_size(WbDeviceTag tag); +const double *wb_accelerometer_get_lookup_table(WbDeviceTag tag); + +// return a pointer to an array of 3 double for X, Y and Z accelerations +const double *wb_accelerometer_get_values(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_ACCELEROMETER_H */ diff --git a/include/controller/c/webots/altimeter.h b/include/controller/c/webots/altimeter.h new file mode 100644 index 0000000..e791132 --- /dev/null +++ b/include/controller/c/webots/altimeter.h @@ -0,0 +1,41 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Altimeter node */ +/**********************************************************************************/ + +#ifndef WB_ALTIMETER_H +#define WB_ALTIMETER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_altimeter_enable(WbDeviceTag tag, int sampling_period); +void wb_altimeter_disable(WbDeviceTag tag); +int wb_altimeter_get_sampling_period(WbDeviceTag tag); + +double wb_altimeter_get_value(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_ALTIMETER_H */ diff --git a/include/controller/c/webots/brake.h b/include/controller/c/webots/brake.h new file mode 100644 index 0000000..7d035d9 --- /dev/null +++ b/include/controller/c/webots/brake.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the brake node */ +/**********************************************************************************/ + +#ifndef WB_BRAKE_H +#define WB_BRAKE_H + +#define WB_USING_C_API +#include "types.h" + +#ifndef WB_MATLAB_LOADLIBRARY +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_brake_set_damping_constant(WbDeviceTag tag, double damping_constant); +WbJointType wb_brake_get_type(WbDeviceTag tag); +WbDeviceTag wb_brake_get_motor(WbDeviceTag tag); +WbDeviceTag wb_brake_get_position_sensor(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_BRAKE_H */ diff --git a/include/controller/c/webots/camera.h b/include/controller/c/webots/camera.h new file mode 100644 index 0000000..fb64e44 --- /dev/null +++ b/include/controller/c/webots/camera.h @@ -0,0 +1,104 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Camera node */ +/**********************************************************************************/ + +#ifndef WB_CAMERA_H +#define WB_CAMERA_H + +#define WB_USING_C_API +#include "camera_recognition_object.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_camera_enable(WbDeviceTag tag, int sampling_period); +void wb_camera_disable(WbDeviceTag tag); +int wb_camera_get_sampling_period(WbDeviceTag tag); + +const unsigned char *wb_camera_get_image(WbDeviceTag tag); +int wb_camera_get_width(WbDeviceTag tag); +int wb_camera_get_height(WbDeviceTag tag); +double wb_camera_get_fov(WbDeviceTag tag); +double wb_camera_get_max_fov(WbDeviceTag tag); +double wb_camera_get_min_fov(WbDeviceTag tag); +void wb_camera_set_fov(WbDeviceTag tag, double fov); // fov specified in rad +double wb_camera_get_exposure(WbDeviceTag tag); +void wb_camera_set_exposure(WbDeviceTag tag, double exposure); +double wb_camera_get_focal_length(WbDeviceTag tag); +double wb_camera_get_focal_distance(WbDeviceTag tag); +double wb_camera_get_max_focal_distance(WbDeviceTag tag); +double wb_camera_get_min_focal_distance(WbDeviceTag tag); +void wb_camera_set_focal_distance(WbDeviceTag tag, double focal_distance); +double wb_camera_get_near(WbDeviceTag tag); +int wb_camera_save_image(WbDeviceTag tag, const char *filename, int quality); + +// smart camera +bool wb_camera_has_recognition(WbDeviceTag tag); +void wb_camera_recognition_enable(WbDeviceTag tag, int sampling_period); +void wb_camera_recognition_disable(WbDeviceTag tag); +int wb_camera_recognition_get_sampling_period(WbDeviceTag tag); +int wb_camera_recognition_get_number_of_objects(WbDeviceTag tag); +const WbCameraRecognitionObject *wb_camera_recognition_get_objects(WbDeviceTag tag); +bool wb_camera_recognition_has_segmentation(WbDeviceTag tag); +void wb_camera_recognition_enable_segmentation(WbDeviceTag tag); +void wb_camera_recognition_disable_segmentation(WbDeviceTag tag); +bool wb_camera_recognition_is_segmentation_enabled(WbDeviceTag tag); +const unsigned char *wb_camera_recognition_get_segmentation_image(WbDeviceTag tag); +int wb_camera_recognition_save_segmentation_image(WbDeviceTag tag, const char *filename, int quality); + +#ifdef WB_MATLAB_LOADLIBRARY +// This function should be used only in the Matlab wrapper +const WbCameraRecognitionObject *wb_camera_recognition_get_object(WbDeviceTag tag, int index); +#endif + +/* useful macros to get pixel colors from the image data, width and coords * + * + * ^ y + * | (height) + * |=============== + * |=============== *: pixel@(x,y) + * |---------*===== + * |=========|===== (width) + * |=========|===== + * |=========|===== + * |=========|===== + * -+-----------------> x + * o| + */ + +#define wb_camera_image_get_red(image, width, x, y) (image[4 * ((y) * (width) + (x)) + 2]) +#define wb_camera_image_get_green(image, width, x, y) (image[4 * ((y) * (width) + (x)) + 1]) +#define wb_camera_image_get_blue(image, width, x, y) (image[4 * ((y) * (width) + (x))]) + +#ifdef KROS_COMPILATION +#define wb_camera_image_get_gray(image, width, x, y) (image[(y) * (width) + (x)]) +#else +#define wb_camera_image_get_gray(image, w, x, y) \ + ((image[4 * ((y) * (w) + (x)) + 2] + image[4 * ((y) * (w) + (x)) + 1] + image[4 * ((y) * (w) + (x))]) / 3) +#endif +// alias +#define wb_camera_image_get_grey(image, width, x, y) wb_camera_image_get_gray(image, width, x, y) + +#ifdef __cplusplus +} +#endif + +#endif /* WB_CAMERA_H */ diff --git a/include/controller/c/webots/camera_recognition_object.h b/include/controller/c/webots/camera_recognition_object.h new file mode 100644 index 0000000..19d0c33 --- /dev/null +++ b/include/controller/c/webots/camera_recognition_object.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Common definition of the 'WbCameraRecognitionObject' for both the C and C++ APIs */ +/**************************************************************************************************/ + +#ifndef WB_CAMERA_RECOGNITION_OBJECT_H +#define WB_CAMERA_RECOGNITION_OBJECT_H + +typedef struct { + int id; + double position[3]; + double orientation[4]; + double size[2]; + int position_on_image[2]; + int size_on_image[2]; + int number_of_colors; + double *colors; + char *model; +} WbCameraRecognitionObject; + +#endif /* WB_CAMERA_RECOGNITION_OBJECT_H */ diff --git a/include/controller/c/webots/compass.h b/include/controller/c/webots/compass.h new file mode 100644 index 0000000..ee1f7b2 --- /dev/null +++ b/include/controller/c/webots/compass.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Compass node */ +/**********************************************************************************/ + +#ifndef WB_COMPASS_H +#define WB_COMPASS_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_compass_enable(WbDeviceTag tag, int sampling_period); +void wb_compass_disable(WbDeviceTag tag); +int wb_compass_get_sampling_period(WbDeviceTag tag); + +int wb_compass_get_lookup_table_size(WbDeviceTag tag); +const double *wb_compass_get_lookup_table(WbDeviceTag tag); + +const double *wb_compass_get_values(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_COMPASS_H */ diff --git a/include/controller/c/webots/connector.h b/include/controller/c/webots/connector.h new file mode 100644 index 0000000..09226ab --- /dev/null +++ b/include/controller/c/webots/connector.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Connector node */ +/**********************************************************************************/ + +#ifndef WB_CONNECTOR_H +#define WB_CONNECTOR_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_connector_enable_presence(WbDeviceTag tag, int sampling_period); +void wb_connector_disable_presence(WbDeviceTag tag); +int wb_connector_get_presence_sampling_period(WbDeviceTag tag); +int wb_connector_get_presence(WbDeviceTag tag); +void wb_connector_lock(WbDeviceTag tag); +void wb_connector_unlock(WbDeviceTag tag); +bool wb_connector_is_locked(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_CONNECTOR_H */ diff --git a/include/controller/c/webots/console.h b/include/controller/c/webots/console.h new file mode 100644 index 0000000..88888f3 --- /dev/null +++ b/include/controller/c/webots/console.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WB_CONSOLE_H +#define WB_CONSOLE_H + +#define WB_USING_C_API + +#ifdef __cplusplus +extern "C" { +#endif + +#define WB_STDOUT 1 +#define WB_STDERR 2 + +// this function is for an internal purpose, please use regular stdout/stderr functions instead +void wb_console_print(const char *text, int stream); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_CONSOLE_H */ diff --git a/include/controller/c/webots/contact_point.h b/include/controller/c/webots/contact_point.h new file mode 100644 index 0000000..9712f3f --- /dev/null +++ b/include/controller/c/webots/contact_point.h @@ -0,0 +1,26 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WB_CONTACT_POINTS_H +#define WB_CONTACT_POINTS_H + +typedef struct { + double point[3]; + int node_id; + int padding; // this padding is anyhow added by the compiler, we make it explicit here +} WbContactPoint; + +#endif diff --git a/include/controller/c/webots/device.h b/include/controller/c/webots/device.h new file mode 100644 index 0000000..5c97ef2 --- /dev/null +++ b/include/controller/c/webots/device.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Abstraction of a robot device */ +/**********************************************************************************/ + +#ifndef WB_DEVICE_H +#define WB_DEVICE_H + +#define WB_USING_C_API +#include "nodes.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +const char *wb_device_get_name(WbDeviceTag dt); +const char *wb_device_get_model(WbDeviceTag dt); +WbNodeType wb_device_get_node_type(WbDeviceTag dt); + +// deprecated since Webots 8.0.0, please use wb_device_get_node_type() instead +WbNodeType wb_device_get_type(WbDeviceTag dt) WB_DEPRECATED; + +#ifdef __cplusplus +} +#endif + +#endif /* WB_DEVICE_H */ diff --git a/include/controller/c/webots/display.h b/include/controller/c/webots/display.h new file mode 100644 index 0000000..7e4580c --- /dev/null +++ b/include/controller/c/webots/display.h @@ -0,0 +1,72 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Display node */ +/**********************************************************************************/ + +#ifndef WB_DISPLAY_H +#define WB_DISPLAY_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +int wb_display_get_width(WbDeviceTag tag); +int wb_display_get_height(WbDeviceTag tag); + +// drawing properties +void wb_display_set_color(WbDeviceTag tag, int color); +void wb_display_set_alpha(WbDeviceTag tag, double alpha); +void wb_display_set_opacity(WbDeviceTag tag, double opacity); +void wb_display_set_font(WbDeviceTag tag, const char *font, int size, bool anti_aliasing); + +void wb_display_attach_camera(WbDeviceTag tag, WbDeviceTag camera_tag); +void wb_display_detach_camera(WbDeviceTag tag); + +// draw primitive +void wb_display_draw_pixel(WbDeviceTag tag, int x, int y); +void wb_display_draw_line(WbDeviceTag tag, int x1, int y1, int x2, int y2); +void wb_display_draw_rectangle(WbDeviceTag tag, int x, int y, int width, int height); +void wb_display_draw_oval(WbDeviceTag tag, int cx, int cy, int a, int b); +void wb_display_draw_polygon(WbDeviceTag tag, const int *x, const int *y, int size); +void wb_display_draw_text(WbDeviceTag tag, const char *text, int x, int y); +void wb_display_fill_rectangle(WbDeviceTag tag, int x, int y, int width, int height); +void wb_display_fill_oval(WbDeviceTag tag, int cx, int cy, int a, int b); +void wb_display_fill_polygon(WbDeviceTag tag, const int *x, const int *y, int size); + +// WbImageRef handle functions +#define WB_IMAGE_RGB 3 +#define WB_IMAGE_RGBA 4 +#define WB_IMAGE_ARGB 5 +#define WB_IMAGE_BGRA 6 +#define WB_IMAGE_ABGR 7 + +WbImageRef wb_display_image_new(WbDeviceTag tag, int width, int height, const void *data, int format); +WbImageRef wb_display_image_copy(WbDeviceTag tag, int x, int y, int width, int height); +WbImageRef wb_display_image_load(WbDeviceTag tag, const char *filename); +void wb_display_image_delete(WbDeviceTag tag, WbImageRef ir); +void wb_display_image_paste(WbDeviceTag tag, WbImageRef ir, int x, int y, bool blend); +void wb_display_image_save(WbDeviceTag tag, WbImageRef ir, const char *filename); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_DISPLAY_H */ diff --git a/include/controller/c/webots/distance_sensor.h b/include/controller/c/webots/distance_sensor.h new file mode 100644 index 0000000..078a1a4 --- /dev/null +++ b/include/controller/c/webots/distance_sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the DistanceSensor node */ +/**********************************************************************************/ + +#ifndef WB_DISTANCE_SENSOR_H +#define WB_DISTANCE_SENSOR_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_distance_sensor_enable(WbDeviceTag tag, int sampling_period); +void wb_distance_sensor_disable(WbDeviceTag tag); +int wb_distance_sensor_get_sampling_period(WbDeviceTag tag); +double wb_distance_sensor_get_value(WbDeviceTag tag); + +double wb_distance_sensor_get_max_value(WbDeviceTag tag); +double wb_distance_sensor_get_min_value(WbDeviceTag tag); +double wb_distance_sensor_get_aperture(WbDeviceTag tag); + +int wb_distance_sensor_get_lookup_table_size(WbDeviceTag tag); +const double *wb_distance_sensor_get_lookup_table(WbDeviceTag tag); + +typedef enum { + WB_DISTANCE_SENSOR_GENERIC = 0, + WB_DISTANCE_SENSOR_INFRA_RED, + WB_DISTANCE_SENSOR_SONAR, + WB_DISTANCE_SENSOR_LASER +} WbDistanceSensorType; + +WbDistanceSensorType wb_distance_sensor_get_type(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_DISTANCE_SENSOR_H */ diff --git a/include/controller/c/webots/emitter.h b/include/controller/c/webots/emitter.h new file mode 100644 index 0000000..7f4896c --- /dev/null +++ b/include/controller/c/webots/emitter.h @@ -0,0 +1,46 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Emitter node */ +/**********************************************************************************/ + +#ifndef WB_EMITTER_H +#define WB_EMITTER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef WB_CHANNEL_BROADCAST +#define WB_CHANNEL_BROADCAST -1 +#endif + +int wb_emitter_send(WbDeviceTag tag, const void *data, int size); +int wb_emitter_get_buffer_size(WbDeviceTag tag); +void wb_emitter_set_channel(WbDeviceTag tag, int channel); +int wb_emitter_get_channel(WbDeviceTag tag); +double wb_emitter_get_range(WbDeviceTag tag); +void wb_emitter_set_range(WbDeviceTag tag, double range); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_EMITTER_H */ diff --git a/include/controller/c/webots/gps.h b/include/controller/c/webots/gps.h new file mode 100644 index 0000000..cb70fda --- /dev/null +++ b/include/controller/c/webots/gps.h @@ -0,0 +1,49 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the GPS node */ +/**********************************************************************************/ + +#ifndef WB_GPS_H +#define WB_GPS_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_gps_enable(WbDeviceTag tag, int sampling_period); +void wb_gps_disable(WbDeviceTag tag); +int wb_gps_get_sampling_period(WbDeviceTag tag); + +double wb_gps_get_speed(WbDeviceTag tag); +const double *wb_gps_get_speed_vector(WbDeviceTag tag); +const double *wb_gps_get_values(WbDeviceTag tag); + +const char *wb_gps_convert_to_degrees_minutes_seconds(double decimal_degrees); + +typedef enum { WB_GPS_LOCAL_COORDINATE = 0, WB_GPS_WGS84_COORDINATE } WbGpsCoordinateSystem; + +WbGpsCoordinateSystem wb_gps_get_coordinate_system(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_GPS_H */ diff --git a/include/controller/c/webots/gyro.h b/include/controller/c/webots/gyro.h new file mode 100644 index 0000000..508e420 --- /dev/null +++ b/include/controller/c/webots/gyro.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Gyro node */ +/**********************************************************************************/ + +#ifndef WB_GYRO_H +#define WB_GYRO_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_gyro_enable(WbDeviceTag tag, int sampling_period); +void wb_gyro_disable(WbDeviceTag tag); +int wb_gyro_get_sampling_period(WbDeviceTag tag); + +int wb_gyro_get_lookup_table_size(WbDeviceTag tag); +const double *wb_gyro_get_lookup_table(WbDeviceTag tag); + +const double *wb_gyro_get_values(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_GYRO_H */ diff --git a/include/controller/c/webots/inertial_unit.h b/include/controller/c/webots/inertial_unit.h new file mode 100644 index 0000000..aeec8db --- /dev/null +++ b/include/controller/c/webots/inertial_unit.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the InertialUnit node */ +/**********************************************************************************/ + +#ifndef WB_INERTIAL_UNIT_H +#define WB_INERTIAL_UNIT_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_inertial_unit_enable(WbDeviceTag tag, int sampling_period); +void wb_inertial_unit_disable(WbDeviceTag tag); +int wb_inertial_unit_get_sampling_period(WbDeviceTag tag); + +double wb_inertial_unit_get_noise(WbDeviceTag tag); + +const double *wb_inertial_unit_get_roll_pitch_yaw(WbDeviceTag tag); +const double *wb_inertial_unit_get_quaternion(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_INERTIAL_UNIT_H */ diff --git a/include/controller/c/webots/joystick.h b/include/controller/c/webots/joystick.h new file mode 100644 index 0000000..2f39d5d --- /dev/null +++ b/include/controller/c/webots/joystick.h @@ -0,0 +1,57 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the computer joystick */ +/**********************************************************************************/ + +#ifndef WB_JOYSTICK_H +#define WB_JOYSTICK_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_joystick_enable(int sampling_period); +void wb_joystick_disable(); +int wb_joystick_get_sampling_period(); + +bool wb_joystick_is_connected(); +const char *wb_joystick_get_model(); + +int wb_joystick_get_number_of_axes(); +int wb_joystick_get_axis_value(int axis); + +int wb_joystick_get_number_of_povs(); +int wb_joystick_get_pov_value(int pov); + +int wb_joystick_get_pressed_button(); + +// force-feedback +void wb_joystick_set_constant_force(int level); +void wb_joystick_set_constant_force_duration(double duration); +void wb_joystick_set_auto_centering_gain(double gain); +void wb_joystick_set_resistance_gain(double gain); +void wb_joystick_set_force_axis(int axis); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_JOYSTICK_H */ diff --git a/include/controller/c/webots/keyboard.h b/include/controller/c/webots/keyboard.h new file mode 100644 index 0000000..18f9749 --- /dev/null +++ b/include/controller/c/webots/keyboard.h @@ -0,0 +1,61 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the computer keyboard */ +/**********************************************************************************/ + +#ifndef WB_KEYBOARD_H +#define WB_KEYBOARD_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +enum { + WB_KEYBOARD_END = 312, + WB_KEYBOARD_HOME, + WB_KEYBOARD_LEFT, + WB_KEYBOARD_UP, + WB_KEYBOARD_RIGHT, + WB_KEYBOARD_DOWN, + WB_KEYBOARD_PAGEUP = 366, + WB_KEYBOARD_PAGEDOWN, + WB_KEYBOARD_NUMPAD_HOME = 375, + WB_KEYBOARD_NUMPAD_LEFT, + WB_KEYBOARD_NUMPAD_UP, + WB_KEYBOARD_NUMPAD_RIGHT, + WB_KEYBOARD_NUMPAD_DOWN, + WB_KEYBOARD_NUMPAD_END = 382, + WB_KEYBOARD_KEY = 0x0000ffff, + WB_KEYBOARD_SHIFT = 0x00010000, + WB_KEYBOARD_CONTROL = 0x00020000, + WB_KEYBOARD_ALT = 0x00040000 +}; + +void wb_keyboard_enable(int sampling_period); +void wb_keyboard_disable(); +int wb_keyboard_get_sampling_period(); +int wb_keyboard_get_key(); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_KEYBOARD_H */ diff --git a/include/controller/c/webots/led.h b/include/controller/c/webots/led.h new file mode 100644 index 0000000..757cc9c --- /dev/null +++ b/include/controller/c/webots/led.h @@ -0,0 +1,38 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the LED node */ +/**********************************************************************************/ + +#ifndef WB_LED_H +#define WB_LED_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_led_set(WbDeviceTag tag, int value); // 0 for off, 1, 2, etc. for on with a color +int wb_led_get(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_LED_H */ diff --git a/include/controller/c/webots/lidar.h b/include/controller/c/webots/lidar.h new file mode 100644 index 0000000..10e8cac --- /dev/null +++ b/include/controller/c/webots/lidar.h @@ -0,0 +1,66 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Lidar node */ +/**********************************************************************************/ + +#ifndef WB_LIDAR_H +#define WB_LIDAR_H + +#define WB_USING_C_API +#include "lidar_point.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_lidar_enable(WbDeviceTag tag, int sampling_period); +void wb_lidar_enable_point_cloud(WbDeviceTag tag); +void wb_lidar_disable(WbDeviceTag tag); +void wb_lidar_disable_point_cloud(WbDeviceTag tag); +int wb_lidar_get_sampling_period(WbDeviceTag tag); +bool wb_lidar_is_point_cloud_enabled(WbDeviceTag tag); + +const float *wb_lidar_get_range_image(WbDeviceTag tag); +const float *wb_lidar_get_layer_range_image(WbDeviceTag tag, int layer); + +const WbLidarPoint *wb_lidar_get_point_cloud(WbDeviceTag tag); +const WbLidarPoint *wb_lidar_get_layer_point_cloud(WbDeviceTag tag, int layer); +int wb_lidar_get_number_of_points(WbDeviceTag tag); + +int wb_lidar_get_horizontal_resolution(WbDeviceTag tag); +int wb_lidar_get_number_of_layers(WbDeviceTag tag); +double wb_lidar_get_min_frequency(WbDeviceTag tag); +double wb_lidar_get_max_frequency(WbDeviceTag tag); +double wb_lidar_get_frequency(WbDeviceTag tag); +void wb_lidar_set_frequency(WbDeviceTag tag, double frequency); +double wb_lidar_get_fov(WbDeviceTag tag); +double wb_lidar_get_vertical_fov(WbDeviceTag tag); +double wb_lidar_get_min_range(WbDeviceTag tag); +double wb_lidar_get_max_range(WbDeviceTag tag); + +#ifdef WB_MATLAB_LOADLIBRARY +// This function should be used only in the Matlab wrapper +const WbLidarPoint *wb_lidar_get_point(WbDeviceTag tag, int index); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* WB_LIDAR_H */ diff --git a/include/controller/c/webots/lidar_point.h b/include/controller/c/webots/lidar_point.h new file mode 100644 index 0000000..b7922c3 --- /dev/null +++ b/include/controller/c/webots/lidar_point.h @@ -0,0 +1,32 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Common definition of the 'WbLidarPoint' for both the C and C++ APIs */ +/*************************************************************************************/ + +#ifndef WB_LIDAR_POINT_H +#define WB_LIDAR_POINT_H + +typedef struct { + float x; + float y; + float z; + int layer_id; + float time; +} WbLidarPoint; + +#endif /* WB_LIDAR_POINT_H */ diff --git a/include/controller/c/webots/light_sensor.h b/include/controller/c/webots/light_sensor.h new file mode 100644 index 0000000..1afbe38 --- /dev/null +++ b/include/controller/c/webots/light_sensor.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the LightSensor node */ +/**********************************************************************************/ + +#ifndef WB_LIGHT_SENSOR_H +#define WB_LIGHT_SENSOR_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_light_sensor_enable(WbDeviceTag tag, int sampling_period); +void wb_light_sensor_disable(WbDeviceTag tag); +int wb_light_sensor_get_sampling_period(WbDeviceTag tag); + +int wb_light_sensor_get_lookup_table_size(WbDeviceTag tag); +const double *wb_light_sensor_get_lookup_table(WbDeviceTag tag); + +double wb_light_sensor_get_value(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_LIGHT_SENSOR_H */ diff --git a/include/controller/c/webots/microphone.h b/include/controller/c/webots/microphone.h new file mode 100644 index 0000000..15fd263 --- /dev/null +++ b/include/controller/c/webots/microphone.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Microphone node */ +/**********************************************************************************/ + +#ifndef WB_MICROPHONE_H +#define WB_MICROPHONE_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// device functions +void wb_microphone_enable(WbDeviceTag tag, int sampling_period); +void wb_microphone_disable(WbDeviceTag tag); +int wb_microphone_get_sampling_period(WbDeviceTag tag); + +// data packet functions +const void *wb_microphone_get_sample_data(WbDeviceTag tag); +int wb_microphone_get_sample_size(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_MICROPHONE_H */ diff --git a/include/controller/c/webots/motor.h b/include/controller/c/webots/motor.h new file mode 100644 index 0000000..8b9ab09 --- /dev/null +++ b/include/controller/c/webots/motor.h @@ -0,0 +1,75 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Motor node */ +/**********************************************************************************/ + +#ifndef WB_MOTOR_H +#define WB_MOTOR_H + +#define WB_USING_C_API +#include "types.h" + +#ifndef WB_MATLAB_LOADLIBRARY +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_motor_set_position(WbDeviceTag tag, double position); // rad or meters +void wb_motor_set_acceleration(WbDeviceTag tag, double acceleration); // rad/s^2 or m/s^2 +void wb_motor_set_velocity(WbDeviceTag tag, double velocity); // rad/s or m/s +void wb_motor_set_force(WbDeviceTag tag, double force); // N +void wb_motor_set_torque(WbDeviceTag tag, double torque); // N*m +void wb_motor_set_available_force(WbDeviceTag tag, double force); // N +void wb_motor_set_available_torque(WbDeviceTag tag, double torque); // N*m +void wb_motor_set_control_pid(WbDeviceTag tag, double p, double i, double d); // set the PID control parameters + +void wb_motor_enable_force_feedback(WbDeviceTag tag, int sampling_period); +void wb_motor_disable_force_feedback(WbDeviceTag tag); +int wb_motor_get_force_feedback_sampling_period(WbDeviceTag tag); +double wb_motor_get_force_feedback(WbDeviceTag tag); + +void wb_motor_enable_torque_feedback(WbDeviceTag tag, int sampling_period); +void wb_motor_disable_torque_feedback(WbDeviceTag tag); +int wb_motor_get_torque_feedback_sampling_period(WbDeviceTag tag); +double wb_motor_get_torque_feedback(WbDeviceTag tag); + +WbJointType wb_motor_get_type(WbDeviceTag tag); + +double wb_motor_get_target_position(WbDeviceTag tag); +double wb_motor_get_min_position(WbDeviceTag tag); +double wb_motor_get_max_position(WbDeviceTag tag); +double wb_motor_get_velocity(WbDeviceTag tag); +double wb_motor_get_max_velocity(WbDeviceTag tag); +double wb_motor_get_acceleration(WbDeviceTag tag); +double wb_motor_get_available_force(WbDeviceTag tag); +double wb_motor_get_max_force(WbDeviceTag tag); +double wb_motor_get_available_torque(WbDeviceTag tag); +double wb_motor_get_max_torque(WbDeviceTag tag); +double wb_motor_get_multiplier(WbDeviceTag tag); + +WbDeviceTag wb_motor_get_brake(WbDeviceTag tag); +WbDeviceTag wb_motor_get_position_sensor(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_MOTOR_H */ diff --git a/include/controller/c/webots/mouse.h b/include/controller/c/webots/mouse.h new file mode 100644 index 0000000..14ab8f4 --- /dev/null +++ b/include/controller/c/webots/mouse.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the computer mouse. */ +/************************************************************************/ + +#ifndef WB_MOUSE_H +#define WB_MOUSE_H + +#define WB_USING_C_API +#include "mouse_state.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_mouse_enable(int sampling_period); +void wb_mouse_disable(); +int wb_mouse_get_sampling_period(); + +void wb_mouse_enable_3d_position(); +void wb_mouse_disable_3d_position(); +bool wb_mouse_is_3d_position_enabled(); + +#ifndef WB_MATLAB_LOADLIBRARY +WbMouseState wb_mouse_get_state(); +#else +// This function should be used only in the Matlab wrapper +WbMouseState *wb_mouse_get_state_pointer(); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* WB_MOUSE_H */ diff --git a/include/controller/c/webots/mouse_state.h b/include/controller/c/webots/mouse_state.h new file mode 100644 index 0000000..8cb038e --- /dev/null +++ b/include/controller/c/webots/mouse_state.h @@ -0,0 +1,40 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Common definition of the 'WbMouseState' for both the C and C++ APIs. */ +/**************************************************************************************/ + +#ifndef WB_MOUSE_STATE_H +#define WB_MOUSE_STATE_H + +#include "types.h" + +typedef struct { + // mouse 2D position in the 3D window + double u; + double v; + // mouse 3D position + double x; + double y; + double z; + // mouse buttons state + bool left; + bool middle; + bool right; +} WbMouseState; + +#endif /* WB_MOUSE_STATE_H */ diff --git a/include/controller/c/webots/nodes.h b/include/controller/c/webots/nodes.h new file mode 100644 index 0000000..7a7b1a0 --- /dev/null +++ b/include/controller/c/webots/nodes.h @@ -0,0 +1,122 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WB_NODES_H +#define WB_NODES_H + +// IMPORTANT: any modification of this file must also be propagated to: +// 1. include/controller/cpp/webots/Node.hpp +// 2. lib/matlab/mgenerate.py + +typedef enum { + WB_NODE_NO_NODE, + // 3D rendering + WB_NODE_APPEARANCE, + WB_NODE_BACKGROUND, + WB_NODE_BILLBOARD, + WB_NODE_BOX, + WB_NODE_CAD_SHAPE, + WB_NODE_CAPSULE, + WB_NODE_COLOR, + WB_NODE_CONE, + WB_NODE_COORDINATE, + WB_NODE_CYLINDER, + WB_NODE_DIRECTIONAL_LIGHT, + WB_NODE_ELEVATION_GRID, + WB_NODE_FOG, + WB_NODE_GROUP, + WB_NODE_IMAGE_TEXTURE, + WB_NODE_INDEXED_FACE_SET, + WB_NODE_INDEXED_LINE_SET, + WB_NODE_MATERIAL, + WB_NODE_MESH, + WB_NODE_MUSCLE, + WB_NODE_NORMAL, + WB_NODE_PBR_APPEARANCE, + WB_NODE_PLANE, + WB_NODE_POINT_LIGHT, + WB_NODE_POINT_SET, + WB_NODE_POSE, + WB_NODE_SHAPE, + WB_NODE_SPHERE, + WB_NODE_SPOT_LIGHT, + WB_NODE_TEXTURE_COORDINATE, + WB_NODE_TEXTURE_TRANSFORM, + WB_NODE_TRANSFORM, + WB_NODE_VIEWPOINT, + // robots + WB_NODE_ROBOT, + // devices + WB_NODE_ACCELEROMETER, + WB_NODE_ALTIMETER, + WB_NODE_BRAKE, + WB_NODE_CAMERA, + WB_NODE_COMPASS, + WB_NODE_CONNECTOR, + WB_NODE_DISPLAY, + WB_NODE_DISTANCE_SENSOR, + WB_NODE_EMITTER, + WB_NODE_GPS, + WB_NODE_GYRO, + WB_NODE_INERTIAL_UNIT, + WB_NODE_LED, + WB_NODE_LIDAR, + WB_NODE_LIGHT_SENSOR, + WB_NODE_LINEAR_MOTOR, + WB_NODE_PEN, + WB_NODE_POSITION_SENSOR, + WB_NODE_PROPELLER, + WB_NODE_RADAR, + WB_NODE_RANGE_FINDER, + WB_NODE_RECEIVER, + WB_NODE_ROTATIONAL_MOTOR, + WB_NODE_SKIN, + WB_NODE_SPEAKER, + WB_NODE_TOUCH_SENSOR, + WB_NODE_VACUUM_GRIPPER, + // misc + WB_NODE_BALL_JOINT, + WB_NODE_BALL_JOINT_PARAMETERS, + WB_NODE_CHARGER, + WB_NODE_CONTACT_PROPERTIES, + WB_NODE_DAMPING, + WB_NODE_FLUID, + WB_NODE_FOCUS, + WB_NODE_HINGE_JOINT, + WB_NODE_HINGE_JOINT_PARAMETERS, + WB_NODE_HINGE_2_JOINT, + WB_NODE_IMMERSION_PROPERTIES, + WB_NODE_JOINT_PARAMETERS, + WB_NODE_LENS, + WB_NODE_LENS_FLARE, + WB_NODE_PHYSICS, + WB_NODE_RECOGNITION, + WB_NODE_SLIDER_JOINT, + WB_NODE_SLOT, + WB_NODE_SOLID, + WB_NODE_SOLID_REFERENCE, + WB_NODE_TRACK, + WB_NODE_TRACK_WHEEL, + WB_NODE_WORLD_INFO, + WB_NODE_ZOOM, + // experimental + WB_NODE_MICROPHONE, + WB_NODE_RADIO +} WbNodeType; + +const char *wb_node_get_name(WbNodeType t); + +#endif /* WB_NODES_H */ diff --git a/include/controller/c/webots/pen.h b/include/controller/c/webots/pen.h new file mode 100644 index 0000000..06526e7 --- /dev/null +++ b/include/controller/c/webots/pen.h @@ -0,0 +1,38 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Pen node */ +/**********************************************************************************/ + +#ifndef WB_PEN_H +#define WB_PEN_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_pen_write(WbDeviceTag tag, bool write); // switch on/off writing +void wb_pen_set_ink_color(WbDeviceTag tag, int color, double density); // set ink color + +#ifdef __cplusplus +} +#endif + +#endif /* WB_PEN_H */ diff --git a/include/controller/c/webots/plugins/robot_window/default.h b/include/controller/c/webots/plugins/robot_window/default.h new file mode 100644 index 0000000..4fe514e --- /dev/null +++ b/include/controller/c/webots/plugins/robot_window/default.h @@ -0,0 +1,41 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C utility programming interface for the default */ +/* robot window. This module is not yet complete as several */ +/* devices are missing for now. More usage examples and */ +/* documentation will be provided once it is complete. */ +/**************************************************************************/ + +#ifndef WBU_DEFAULT_ROBOT_WINDOW_H +#define WBU_DEFAULT_ROBOT_WINDOW_H + +#include "robot_wwi.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wbu_default_robot_window_configure(); +void wbu_default_robot_window_update(); +void wbu_default_robot_window_set_images_max_size(int max_width, int max_height); + +#ifdef __cplusplus +} +#endif + +#endif /* WBU_DEFAULT_ROBOT_WINDOW_H */ diff --git a/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h b/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h new file mode 100644 index 0000000..fdc6ece --- /dev/null +++ b/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C utility to enable/disable devices and retrieve the */ +/* measurements from a robot window */ +/*************************************************************************************/ + +#ifndef WBU_GENERIC_ROBOT_WINDOW_H +#define WBU_GENERIC_ROBOT_WINDOW_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void wbu_generic_robot_window_parse_device_command(char *token, char *tokens); +bool wbu_generic_robot_window_parse_device_control_command(const char *first_token, char *tokens); +bool wbu_generic_robot_window_handle_messages(const char *message); +void wbu_generic_robot_window_init(); +void wbu_generic_robot_window_update(); +bool wbu_generic_robot_window_is_hidden(); +double wbu_generic_robot_window_refresh_rate(); +bool wbu_generic_robot_window_needs_update(); + +#ifdef __cplusplus +} +#endif + +#endif // WBU_GENERIC_ROBOT_WINDOW_H diff --git a/include/controller/c/webots/plugins/robot_window/robot_window.h b/include/controller/c/webots/plugins/robot_window/robot_window.h new file mode 100644 index 0000000..4987ec6 --- /dev/null +++ b/include/controller/c/webots/plugins/robot_window/robot_window.h @@ -0,0 +1,30 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WB_ROBOT_WINDOW_H +#define WB_ROBOT_WINDOW_H + +#ifdef __cplusplus +extern "C" { +#endif + +void *wb_robot_window_custom_function(void *); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_ROBOT_WINDOW_H */ diff --git a/include/controller/c/webots/plugins/robot_window/robot_wwi.h b/include/controller/c/webots/plugins/robot_window/robot_wwi.h new file mode 100644 index 0000000..29e253d --- /dev/null +++ b/include/controller/c/webots/plugins/robot_window/robot_wwi.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WB_ROBOT_WWI_H +#define WB_ROBOT_WWI_H + +#ifdef __cplusplus +#include +extern "C" { +#else +#include +#endif + +void wb_robot_wwi_send(const char *data, int size); +const char *wb_robot_wwi_receive(int *size); +const char *wb_robot_wwi_receive_text(); +#define wb_robot_wwi_send_text(t) wb_robot_wwi_send(t, strlen(t) + 1) + +#ifdef __cplusplus +} +#endif + +#endif /* WB_ROBOT_WWI_H */ diff --git a/include/controller/c/webots/position_sensor.h b/include/controller/c/webots/position_sensor.h new file mode 100644 index 0000000..49504cc --- /dev/null +++ b/include/controller/c/webots/position_sensor.h @@ -0,0 +1,47 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Motor node */ +/**********************************************************************************/ + +#ifndef WB_POSITION_SENSOR_H +#define WB_POSITION_SENSOR_H + +#define WB_USING_C_API +#include "types.h" + +#ifndef WB_MATLAB_LOADLIBRARY +#include +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_position_sensor_enable(WbDeviceTag tag, int sampling_period); // milliseconds +void wb_position_sensor_disable(WbDeviceTag tag); +int wb_position_sensor_get_sampling_period(WbDeviceTag tag); +double wb_position_sensor_get_value(WbDeviceTag tag); // rad or meters +WbJointType wb_position_sensor_get_type(WbDeviceTag tag); +WbDeviceTag wb_position_sensor_get_motor(WbDeviceTag tag); +WbDeviceTag wb_position_sensor_get_brake(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_POSITION_SENSOR_H */ diff --git a/include/controller/c/webots/radar.h b/include/controller/c/webots/radar.h new file mode 100644 index 0000000..87d684f --- /dev/null +++ b/include/controller/c/webots/radar.h @@ -0,0 +1,53 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Radar node */ +/**********************************************************************************/ + +#ifndef WB_RADAR_H +#define WB_RADAR_H + +#define WB_USING_C_API +#include "radar_target.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_radar_enable(WbDeviceTag tag, int sampling_period); +void wb_radar_disable(WbDeviceTag tag); +int wb_radar_get_sampling_period(WbDeviceTag tag); + +int wb_radar_get_number_of_targets(WbDeviceTag tag); +const WbRadarTarget *wb_radar_get_targets(WbDeviceTag tag); + +double wb_radar_get_min_range(WbDeviceTag tag); +double wb_radar_get_max_range(WbDeviceTag tag); +double wb_radar_get_horizontal_fov(WbDeviceTag tag); +double wb_radar_get_vertical_fov(WbDeviceTag tag); + +#ifdef WB_MATLAB_LOADLIBRARY +// This function should be used only in the Matlab wrapper +const WbRadarTarget *wb_radar_get_target(WbDeviceTag tag, int index); +#endif + +#ifdef __cplusplus +} +#endif + +#endif /* WB_RADAR_H */ diff --git a/include/controller/c/webots/radar_target.h b/include/controller/c/webots/radar_target.h new file mode 100644 index 0000000..5f51a66 --- /dev/null +++ b/include/controller/c/webots/radar_target.h @@ -0,0 +1,31 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Common definition of the 'WbRadarTarget' for both the C and C++ APIs */ +/**************************************************************************************/ + +#ifndef WB_RADAR_TARGET_H +#define WB_RADAR_TARGET_H + +typedef struct { + double distance; + double received_power; + double speed; + double azimuth; +} WbRadarTarget; + +#endif /* WB_RADAR_TARGET_H */ diff --git a/include/controller/c/webots/radio.h b/include/controller/c/webots/radio.h new file mode 100644 index 0000000..f41dda4 --- /dev/null +++ b/include/controller/c/webots/radio.h @@ -0,0 +1,75 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Radio node */ +/**********************************************************************************/ + +#ifndef WB_RADIO_H +#define WB_RADIO_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void *WbRadioMessage; +typedef void *WbRadioEvent; + +WbRadioMessage wb_radio_message_new(int length, const char *body, const char *destination); +void wb_radio_message_delete(WbRadioMessage); +const char *wb_radio_message_get_destination(WbRadioMessage); +int wb_radio_message_get_length(WbRadioMessage); +const char *wb_radio_message_get_body(WbRadioMessage); + +void wb_radio_enable(WbDeviceTag tag, int sampling_period); +void wb_radio_disable(WbDeviceTag tag); + +void wb_radio_set_address(WbDeviceTag tag, const char *address); +const char *wb_radio_get_address(WbDeviceTag tag); + +void wb_radio_set_frequency(WbDeviceTag tag, double hz); +double wb_radio_get_frequency(WbDeviceTag tag); + +void wb_radio_set_channel(WbDeviceTag tag, int channel); +int wb_radio_get_channel(WbDeviceTag tag); + +void wb_radio_set_bitrate(WbDeviceTag tag, int bits_per_second); +int wb_radio_get_bitrate(WbDeviceTag tag); + +void wb_radio_set_rx_sensitivity(WbDeviceTag tag, double dBm); +double wb_radio_get_rx_sensitivity(WbDeviceTag tag); + +void wb_radio_set_tx_power(WbDeviceTag tag, double dBm); +double wb_radio_get_tx_power(WbDeviceTag tag); + +void wb_radio_set_callback(WbDeviceTag tag, void (*)(const WbRadioEvent)); + +WbDeviceTag wb_radio_event_get_radio(const WbRadioEvent); +char *wb_radio_event_get_data(const WbRadioEvent); +int wb_radio_event_get_data_size(const WbRadioEvent); +char *wb_radio_event_get_emitter(const WbRadioEvent); +double wb_radio_event_get_rssi(const WbRadioEvent); + +void wb_radio_send(WbDeviceTag tag, const WbRadioMessage, double delay); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_RADIO_H */ diff --git a/include/controller/c/webots/range_finder.h b/include/controller/c/webots/range_finder.h new file mode 100644 index 0000000..2f9ac41 --- /dev/null +++ b/include/controller/c/webots/range_finder.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the RangeFinder node */ +/**********************************************************************************/ + +#ifndef WB_RANGE_FINDER_H +#define WB_RANGE_FINDER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_range_finder_enable(WbDeviceTag tag, int sampling_period); +void wb_range_finder_disable(WbDeviceTag tag); +int wb_range_finder_get_sampling_period(WbDeviceTag tag); + +const float *wb_range_finder_get_range_image(WbDeviceTag tag); +int wb_range_finder_get_width(WbDeviceTag tag); +int wb_range_finder_get_height(WbDeviceTag tag); +double wb_range_finder_get_fov(WbDeviceTag tag); +double wb_range_finder_get_min_range(WbDeviceTag tag); +double wb_range_finder_get_max_range(WbDeviceTag tag); +int wb_range_finder_save_image(WbDeviceTag tag, const char *filename, int quality); + +// range finder functions +#define wb_range_finder_image_get_depth(image, width, x, y) (image[(y) * (width) + (x)]) + +#ifdef __cplusplus +} +#endif + +#endif /* WB_RANGE_FINDER_H */ diff --git a/include/controller/c/webots/receiver.h b/include/controller/c/webots/receiver.h new file mode 100644 index 0000000..d2ae912 --- /dev/null +++ b/include/controller/c/webots/receiver.h @@ -0,0 +1,53 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Receiver node */ +/**********************************************************************************/ + +#ifndef WB_RECEIVER_H +#define WB_RECEIVER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef WB_CHANNEL_BROADCAST +#define WB_CHANNEL_BROADCAST -1 +#endif + +// device functions +void wb_receiver_enable(WbDeviceTag tag, int sampling_period); +void wb_receiver_disable(WbDeviceTag tag); +int wb_receiver_get_sampling_period(WbDeviceTag tag); + +void wb_receiver_set_channel(WbDeviceTag tag, int channel); +int wb_receiver_get_channel(WbDeviceTag tag); +int wb_receiver_get_queue_length(WbDeviceTag tag); +void wb_receiver_next_packet(WbDeviceTag tag); +int wb_receiver_get_data_size(WbDeviceTag tag); +const void *wb_receiver_get_data(WbDeviceTag tag); +double wb_receiver_get_signal_strength(WbDeviceTag tag); +const double *wb_receiver_get_emitter_direction(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_RECEIVER_H */ diff --git a/include/controller/c/webots/remote_control.h b/include/controller/c/webots/remote_control.h new file mode 100644 index 0000000..32b8553 --- /dev/null +++ b/include/controller/c/webots/remote_control.h @@ -0,0 +1,128 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for communicating with the */ +/* remote control library */ +/**********************************************************************************/ + +#ifndef WB_REMOTE_CONTROL_H +#define WB_REMOTE_CONTROL_H + +#define WB_USING_C_API +#include "camera_recognition_object.h" +#include "radar_target.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void *wb_remote_control_custom_function(void *); + +// Sensor functions (values read by the controller) +void wbr_robot_battery_sensor_set_value(double value); + +void wbr_accelerometer_set_values(WbDeviceTag tag, const double *values); +void wbr_camera_recognition_set_object(WbDeviceTag tag, const WbCameraRecognitionObject *objects, int object_number); +void wbr_compass_set_values(WbDeviceTag tag, const double *values); +void wbr_distance_sensor_set_value(WbDeviceTag tag, double value); +void wbr_gps_set_values(WbDeviceTag tag, const double *values); +void wbr_gps_set_speed(WbDeviceTag tag, const double speed); +void wbr_gps_set_velocity_vector(WbDeviceTag tag, const double *values); +void wbr_gyro_set_values(WbDeviceTag tag, const double *values); +void wbr_inertial_unit_set_value(WbDeviceTag tag, double value); +void wbr_light_sensor_set_value(WbDeviceTag tag, double value); +void wbr_microphone_set_buffer(WbDeviceTag tag, const unsigned char *buffer, int size); +void wbr_motor_set_position_feedback(WbDeviceTag tag, double value); +void wbr_motor_set_force_feedback(WbDeviceTag tag, double value); +void wbr_motor_set_torque_feedback(WbDeviceTag tag, double value); +void wbr_position_sensor_set_value(WbDeviceTag tag, double value); +void wbr_radar_set_targets(WbDeviceTag tag, const WbRadarTarget *targets, int target_number); +void wbr_touch_sensor_set_value(WbDeviceTag tag, double value); +void wbr_touch_sensor_set_values(WbDeviceTag tag, const double *values); + +// TODO doc required +void wbr_display_save_image(WbDeviceTag tag, int id, int width, int height, unsigned char *image); + +void wbr_camera_set_image(WbDeviceTag tag, const unsigned char *image); +unsigned char *wbr_camera_get_image_buffer(WbDeviceTag tag); + +// Actuator functions (values written by the controller) +typedef struct WbrInterface { + // mandatory functions : + struct { + bool (*wbr_start)(const char *); + void (*wbr_stop)(); + bool (*wbr_has_failed)(); + void (*wbr_stop_actuators)(); + int (*wbr_robot_step)(int); + } mandatory; + + // user custom function to communicate data + void *(*wbr_custom_function)(void *); + + // optional functions (if they are used but not defined it will print a warning) : + void (*wbr_robot_battery_set_sampling_period)(int sampling_period); + + void (*wbr_set_sampling_period)(WbDeviceTag tag, int sampling_period); + void (*wbr_camera_set_fov)(WbDeviceTag tag, double fov); + void (*wbr_camera_set_exposure)(WbDeviceTag tag, double exposure); + void (*wbr_camera_set_focal_distance)(WbDeviceTag tag, double focal_distance); + void (*wbr_led_set)(WbDeviceTag tag, int state); + void (*wbr_pen_set_ink_color)(WbDeviceTag tag, int color, double density); + void (*wbr_pen_write)(WbDeviceTag tag, bool write); + void (*wbr_speaker_emit_sample)(WbDeviceTag tag, const void *data, int size); + + void (*wbr_motor_set_force_sampling_period)(WbDeviceTag tag, int sampling_period); + void (*wbr_motor_set_torque_sampling_period)(WbDeviceTag tag, int sampling_period); + void (*wbr_motor_set_position)(WbDeviceTag tag, double position); + void (*wbr_motor_set_acceleration)(WbDeviceTag tag, double acceleration); + void (*wbr_motor_set_velocity)(WbDeviceTag tag, double velocity); + void (*wbr_motor_set_force)(WbDeviceTag tag, double force); + void (*wbr_motor_set_torque)(WbDeviceTag tag, double torque); + void (*wbr_motor_set_available_force)(WbDeviceTag tag, double available_force); + void (*wbr_motor_set_available_torque)(WbDeviceTag tag, double available_torque); + void (*wbr_motor_set_control_pid)(WbDeviceTag tag, double p, double i, double d); + + void (*wbr_display_set_color)(WbDeviceTag tag, int color); + void (*wbr_display_set_alpha)(WbDeviceTag tag, double alpha); + void (*wbr_display_set_opacity)(WbDeviceTag tag, double opacity); + + void (*wbr_display_draw_pixel)(WbDeviceTag tag, int x, int y); + void (*wbr_display_draw_line)(WbDeviceTag tag, int x1, int y1, int x2, int y2); + void (*wbr_display_draw_rectangle)(WbDeviceTag tag, int x, int y, int width, int height); + void (*wbr_display_draw_oval)(WbDeviceTag tag, int cx, int cy, int a, int b); + void (*wbr_display_draw_polygon)(WbDeviceTag tag, const int *x, const int *y, int size); + void (*wbr_display_draw_text)(WbDeviceTag tag, const char *txt, int x, int y); + void (*wbr_display_fill_rectangle)(WbDeviceTag tag, int x, int y, int width, int height); + void (*wbr_display_fill_oval)(WbDeviceTag tag, int cx, int cy, int a, int b); + void (*wbr_display_fill_polygon)(WbDeviceTag tag, const int *x, const int *y, int size); + + // TODO these functions are not like the API, documentation required + void (*wbr_display_image_new)(WbDeviceTag tag, int id, int width, int height, const void *data, int format); + void (*wbr_display_image_copy)(WbDeviceTag tag, int id, int x, int y, int width, int height); + void (*wbr_display_image_delete)(WbDeviceTag tag, int id); + void (*wbr_display_image_paste)(WbDeviceTag tag, int id, int x, int y); + void (*wbr_display_image_save)(WbDeviceTag tag, int id); + +} WbrInterface; + +#ifdef __cplusplus +} +#endif + +#endif /* WB_REMOTE_CONTROL_H */ diff --git a/include/controller/c/webots/robot.h b/include/controller/c/webots/robot.h new file mode 100644 index 0000000..e7849fd --- /dev/null +++ b/include/controller/c/webots/robot.h @@ -0,0 +1,133 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Robot node */ +/**********************************************************************************/ + +#ifndef WB_ROBOT_H +#define WB_ROBOT_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __CYGWIN__ +#include +#endif + +#if defined(__VISUALC__) || defined(_MSC_VER) +#include "stdio.h" +#endif + +#include "nodes.h" + +#ifdef KROS_COMPILATION +#define main() _kros_main() +#endif + +typedef void *WbMutexRef; // identifier of a mutex + +typedef enum { WB_MODE_SIMULATION = 0, WB_MODE_CROSS_COMPILATION, WB_MODE_REMOTE_CONTROL } WbRobotMode; + +typedef enum { + WB_EVENT_QUIT = -1, + WB_EVENT_NO_EVENT = 0, + WB_EVENT_MOUSE_CLICK = 1, + WB_EVENT_MOUSE_MOVE = 2, + WB_EVENT_KEYBOARD = 4, + WB_EVENT_JOYSTICK_BUTTON = 8, + WB_EVENT_JOYSTICK_AXIS = 16, + WB_EVENT_JOYSTICK_POV = 32 +} WbUserInputEvent; + +// cart function headers +#ifdef __cplusplus +extern "C" { +#endif + +#if !defined(__VISUALC__) && !defined(_MSC_VER) +int wb_robot_init(); + +/* In the visual studio case, the buffer size of the standard output and + * the standard error cannot be modified from a dll + */ +#else +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 +// We cannot call fflush from the libController as libController is compiled with gcc8 and won't flush the stdout/stderr +// of a gcc7 (cygwin) compiled binary. Therefore, we need to perform the fflush in a gcc7 compiled code, e.g., in a macro here. +#define wb_robot_step(d) (fflush(NULL), wb_robot_step(d)) +#endif + +WbUserInputEvent wb_robot_wait_for_user_input_event(WbUserInputEvent event_type, int timeout); // milliseconds +void wb_robot_cleanup(); +double wb_robot_get_time(); +const char *wb_robot_get_urdf(const char *prefix); +const char *wb_robot_get_name(); +const char *wb_robot_get_model(); +const char *wb_robot_get_custom_data(); +void wb_robot_set_custom_data(const char *data); +WbRobotMode wb_robot_get_mode(); +void wb_robot_set_mode(WbRobotMode mode, const char *arg); +bool wb_robot_get_synchronization(); +bool wb_robot_get_supervisor(); +const char *wb_robot_get_project_path(); +const char *wb_robot_get_world_path(); +double wb_robot_get_basic_time_step(); +WbDeviceTag wb_robot_get_device(const char *name); + +// Introspection API +int wb_robot_get_number_of_devices(); +WbDeviceTag wb_robot_get_device_by_index(int index); + +// robot battery API +void wb_robot_battery_sensor_enable(int sampling_period); +void wb_robot_battery_sensor_disable(); +int wb_robot_battery_sensor_get_sampling_period(); +double wb_robot_battery_sensor_get_value(); + +// robot multi-thread API +#ifndef WB_MATLAB_LOADLIBRARY +void wb_robot_task_new(void (*task)(void *), void *param); // create a task +WbMutexRef wb_robot_mutex_new(); +void wb_robot_mutex_lock(WbMutexRef); +void wb_robot_mutex_unlock(WbMutexRef); +void wb_robot_mutex_delete(WbMutexRef); +#endif + +// Motion editor specfic function : Please don't use this function outside qt_utils +// This function doesn't work if the robot window has not been shown at lease once +void wb_robot_pin_to_static_environment(bool pin); + +// Deprecated functions +// deprecated since Webots 2018a, please use wb_robot_get_custom_data and +// wb_robot_set_custom_data instead +const char *wb_robot_get_controller_name() WB_DEPRECATED; +const char *wb_robot_get_data() WB_DEPRECATED; +void wb_robot_set_data(const char *data) WB_DEPRECATED; + +#ifdef __cplusplus +} +#endif + +#endif /* WB_ROBOT_H */ diff --git a/include/controller/c/webots/skin.h b/include/controller/c/webots/skin.h new file mode 100644 index 0000000..35275b8 --- /dev/null +++ b/include/controller/c/webots/skin.h @@ -0,0 +1,42 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Skin node */ +/**********************************************************************************/ + +#ifndef WB_SKIN_H +#define WB_SKIN_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_skin_set_bone_orientation(WbDeviceTag tag, int index, const double orientation[4], bool absolute); +void wb_skin_set_bone_position(WbDeviceTag tag, int index, const double position[3], bool absolute); +int wb_skin_get_bone_count(WbDeviceTag tag); +const char *wb_skin_get_bone_name(WbDeviceTag tag, int index); +const double *wb_skin_get_bone_orientation(WbDeviceTag tag, int index, bool absolute); +const double *wb_skin_get_bone_position(WbDeviceTag tag, int index, bool absolute); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_SKIN_H */ diff --git a/include/controller/c/webots/speaker.h b/include/controller/c/webots/speaker.h new file mode 100644 index 0000000..5cdd447 --- /dev/null +++ b/include/controller/c/webots/speaker.h @@ -0,0 +1,49 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Speaker node */ +/**********************************************************************************/ + +#ifndef WB_SPEAKER_H +#define WB_SPEAKER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_speaker_play_sound(WbDeviceTag left, WbDeviceTag right, const char *sound, double volume, double pitch, double balance, + bool loop); +void wb_speaker_stop(WbDeviceTag tag, const char *sound); +bool wb_speaker_is_sound_playing(WbDeviceTag tag, const char *sound); + +// "pico" or "microsoft" (Windows only) +bool wb_speaker_set_engine(WbDeviceTag tag, const char *engine); +// "en-US", "en-UK", "de-DE", "es-ES", "fr-FR", "it-IT", etc. +bool wb_speaker_set_language(WbDeviceTag tag, const char *language); +const char *wb_speaker_get_engine(WbDeviceTag tag); +const char *wb_speaker_get_language(WbDeviceTag tag); +void wb_speaker_speak(WbDeviceTag tag, const char *text, double volume); +bool wb_speaker_is_speaking(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_SPEAKER_H */ diff --git a/include/controller/c/webots/supervisor.h b/include/controller/c/webots/supervisor.h new file mode 100644 index 0000000..092c02c --- /dev/null +++ b/include/controller/c/webots/supervisor.h @@ -0,0 +1,252 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the Supervisor node */ +/**********************************************************************************/ + +#ifndef WB_SUPERVISOR_H +#define WB_SUPERVISOR_H + +#define WB_USING_C_API +#include "contact_point.h" +#include "nodes.h" +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + WB_NO_FIELD = 0x00, + WB_SF_BOOL = 0x01, + WB_SF_INT32, + WB_SF_FLOAT, + WB_SF_VEC2F, + WB_SF_VEC3F, + WB_SF_ROTATION, + WB_SF_COLOR, + WB_SF_STRING, + WB_SF_NODE, + WB_MF = 0x10, + WB_MF_BOOL, + WB_MF_INT32, + WB_MF_FLOAT, + WB_MF_VEC2F, + WB_MF_VEC3F, + WB_MF_ROTATION, + WB_MF_COLOR, + WB_MF_STRING, + WB_MF_NODE +} WbFieldType; + +typedef enum { + WB_SUPERVISOR_SIMULATION_MODE_PAUSE = 0, + WB_SUPERVISOR_SIMULATION_MODE_REAL_TIME, + WB_SUPERVISOR_SIMULATION_MODE_FAST +} WbSimulationMode; + +void wb_supervisor_world_load(const char *filename); +bool wb_supervisor_world_save(const char *filename); +void wb_supervisor_world_reload(); + +void wb_supervisor_simulation_quit(int status); +void wb_supervisor_simulation_reset(); +void wb_supervisor_simulation_reset_physics(); + +WbSimulationMode wb_supervisor_simulation_get_mode(); +void wb_supervisor_simulation_set_mode(WbSimulationMode mode); + +void wb_supervisor_set_label(int id, const char *text, double x, double y, double size, int color, double transparency, + const char *font); + +void wb_supervisor_export_image(const char *filename, int quality); + +void wb_supervisor_movie_start_recording(const char *filename, int width, int height, int codec, int quality, int acceleration, + bool caption); +void wb_supervisor_movie_stop_recording(); +bool wb_supervisor_movie_is_ready(); +bool wb_supervisor_movie_failed(); + +bool wb_supervisor_animation_start_recording(const char *filename); +bool wb_supervisor_animation_stop_recording(); + +WbNodeRef wb_supervisor_node_get_root(); +WbNodeRef wb_supervisor_node_get_self(); +int wb_supervisor_node_get_id(WbNodeRef node); +WbNodeRef wb_supervisor_node_get_from_id(int id); +WbNodeRef wb_supervisor_node_get_from_device(WbDeviceTag tag); +WbNodeRef wb_supervisor_node_get_from_def(const char *def); +WbNodeRef wb_supervisor_node_get_from_proto_def(WbNodeRef node, const char *def); +WbNodeRef wb_supervisor_node_get_parent_node(WbNodeRef node); +WbNodeRef wb_supervisor_node_get_selected(); +WbNodeType wb_supervisor_node_get_type(WbNodeRef node); +WbFieldRef wb_supervisor_node_get_field(WbNodeRef node, const char *field_name); +WbFieldRef wb_supervisor_node_get_field_by_index(WbNodeRef node, const int index); +int wb_supervisor_node_get_number_of_fields(WbNodeRef node); +WbFieldRef wb_supervisor_node_get_base_node_field(WbNodeRef node, const char *field_name); +WbFieldRef wb_supervisor_node_get_base_node_field_by_index(WbNodeRef node, int index); +int wb_supervisor_node_get_number_of_base_node_fields(WbNodeRef node); +void wb_supervisor_node_remove(WbNodeRef node); +void wb_supervisor_node_save_state(WbNodeRef node, const char *state_name); +void wb_supervisor_node_load_state(WbNodeRef node, const char *state_name); +void wb_supervisor_node_set_joint_position(WbNodeRef node, double position, int index); +WbProtoRef wb_supervisor_node_get_proto(WbNodeRef node); + +const char *wb_supervisor_node_get_def(WbNodeRef node); +const char *wb_supervisor_node_get_type_name(WbNodeRef node); +const char *wb_supervisor_node_get_base_type_name(WbNodeRef node); +bool wb_supervisor_node_is_proto(WbNodeRef node); +const double *wb_supervisor_node_get_center_of_mass(WbNodeRef node); + +const double *wb_supervisor_node_get_contact_point(WbNodeRef node, int index); +WbNodeRef wb_supervisor_node_get_contact_point_node(WbNodeRef node, int index); +int wb_supervisor_node_get_number_of_contact_points(WbNodeRef node, bool include_descendants); + +WbContactPoint *wb_supervisor_node_get_contact_points(WbNodeRef node, bool include_descendants, int *size); + +const double *wb_supervisor_node_get_orientation(WbNodeRef node); +const double *wb_supervisor_node_get_position(WbNodeRef node); +const double *wb_supervisor_node_get_pose(WbNodeRef node, WbNodeRef from_node); +bool wb_supervisor_node_get_static_balance(WbNodeRef node); +const double *wb_supervisor_node_get_velocity(WbNodeRef node); +void wb_supervisor_node_set_velocity(WbNodeRef node, const double velocity[6]); +void wb_supervisor_node_reset_physics(WbNodeRef node); +void wb_supervisor_node_restart_controller(WbNodeRef node); +const char *wb_supervisor_node_export_string(WbNodeRef node); + +void wb_supervisor_node_move_viewpoint(WbNodeRef node); + +void wb_supervisor_node_set_visibility(WbNodeRef node, WbNodeRef from, bool visible); + +void wb_supervisor_node_add_force(WbNodeRef node, const double force[3], bool relative); +void wb_supervisor_node_add_force_with_offset(WbNodeRef node, const double force[3], const double offset[3], bool relative); +void wb_supervisor_node_add_torque(WbNodeRef node, const double torque[3], bool relative); + +const char *wb_supervisor_field_get_name(WbFieldRef field); +WbFieldType wb_supervisor_field_get_type(WbFieldRef field); +const char *wb_supervisor_field_get_type_name(WbFieldRef field); +int wb_supervisor_field_get_count(WbFieldRef field); +WbFieldRef wb_supervisor_field_get_actual_field(WbFieldRef field); + +void wb_supervisor_field_enable_sf_tracking(WbFieldRef field, int sampling_period); +void wb_supervisor_field_disable_sf_tracking(WbFieldRef field); +void wb_supervisor_node_enable_pose_tracking(WbNodeRef node, int sampling_period, WbNodeRef from_node); +void wb_supervisor_node_disable_pose_tracking(WbNodeRef node, WbNodeRef from_node); + +void wb_supervisor_node_enable_contact_points_tracking(WbNodeRef node, int sampling_period, bool include_descendants); +void wb_supervisor_node_disable_contact_points_tracking(WbNodeRef node); + +bool wb_supervisor_field_get_sf_bool(WbFieldRef field); +int wb_supervisor_field_get_sf_int32(WbFieldRef field); +double wb_supervisor_field_get_sf_float(WbFieldRef field); +const double *wb_supervisor_field_get_sf_vec2f(WbFieldRef field); +const double *wb_supervisor_field_get_sf_vec3f(WbFieldRef field); +const double *wb_supervisor_field_get_sf_rotation(WbFieldRef field); +const double *wb_supervisor_field_get_sf_color(WbFieldRef field); +const char *wb_supervisor_field_get_sf_string(WbFieldRef field); +WbNodeRef wb_supervisor_field_get_sf_node(WbFieldRef field); + +bool wb_supervisor_field_get_mf_bool(WbFieldRef field, int index); +int wb_supervisor_field_get_mf_int32(WbFieldRef field, int index); +double wb_supervisor_field_get_mf_float(WbFieldRef field, int index); +const double *wb_supervisor_field_get_mf_vec2f(WbFieldRef field, int index); +const double *wb_supervisor_field_get_mf_vec3f(WbFieldRef field, int index); +const double *wb_supervisor_field_get_mf_color(WbFieldRef field, int index); +const double *wb_supervisor_field_get_mf_rotation(WbFieldRef field, int index); +const char *wb_supervisor_field_get_mf_string(WbFieldRef field, int index); +WbNodeRef wb_supervisor_field_get_mf_node(WbFieldRef field, int index); + +void wb_supervisor_field_set_sf_bool(WbFieldRef field, bool value); +void wb_supervisor_field_set_sf_int32(WbFieldRef field, int value); +void wb_supervisor_field_set_sf_float(WbFieldRef field, double value); +void wb_supervisor_field_set_sf_vec2f(WbFieldRef field, const double values[2]); +void wb_supervisor_field_set_sf_vec3f(WbFieldRef field, const double values[3]); +void wb_supervisor_field_set_sf_rotation(WbFieldRef field, const double values[4]); +void wb_supervisor_field_set_sf_color(WbFieldRef field, const double values[3]); +void wb_supervisor_field_set_sf_string(WbFieldRef field, const char *value); + +void wb_supervisor_field_set_mf_bool(WbFieldRef field, int index, bool value); +void wb_supervisor_field_set_mf_int32(WbFieldRef field, int index, int value); +void wb_supervisor_field_set_mf_float(WbFieldRef field, int index, double value); +void wb_supervisor_field_set_mf_vec2f(WbFieldRef field, int index, const double values[2]); +void wb_supervisor_field_set_mf_vec3f(WbFieldRef field, int index, const double values[3]); +void wb_supervisor_field_set_mf_rotation(WbFieldRef field, int index, const double values[4]); +void wb_supervisor_field_set_mf_color(WbFieldRef field, int index, const double values[3]); +void wb_supervisor_field_set_mf_string(WbFieldRef field, int index, const char *value); + +void wb_supervisor_field_insert_mf_bool(WbFieldRef field, int index, bool value); +void wb_supervisor_field_insert_mf_int32(WbFieldRef field, int index, int value); +void wb_supervisor_field_insert_mf_float(WbFieldRef field, int index, double value); +void wb_supervisor_field_insert_mf_vec2f(WbFieldRef field, int index, const double values[2]); +void wb_supervisor_field_insert_mf_vec3f(WbFieldRef field, int index, const double values[3]); +void wb_supervisor_field_insert_mf_rotation(WbFieldRef field, int index, const double values[4]); +void wb_supervisor_field_insert_mf_color(WbFieldRef field, int index, const double values[3]); +void wb_supervisor_field_insert_mf_string(WbFieldRef field, int index, const char *value); +void wb_supervisor_field_remove_mf(WbFieldRef field, int index); +void wb_supervisor_field_import_mf_node_from_string(WbFieldRef field, int position, const char *node_string); +void wb_supervisor_field_remove_sf(WbFieldRef field); +void wb_supervisor_field_import_sf_node_from_string(WbFieldRef field, const char *node_string); + +const char *wb_supervisor_proto_get_type_name(WbProtoRef proto); +bool wb_supervisor_proto_is_derived(WbProtoRef proto); +WbProtoRef wb_supervisor_proto_get_parent(WbProtoRef proto); +WbFieldRef wb_supervisor_proto_get_field(WbProtoRef proto, const char *field_name); +WbFieldRef wb_supervisor_proto_get_field_by_index(WbProtoRef proto, int index); +int wb_supervisor_proto_get_number_of_fields(WbProtoRef proto); + +bool wb_supervisor_virtual_reality_headset_is_used(); +const double *wb_supervisor_virtual_reality_headset_get_position(); +const double *wb_supervisor_virtual_reality_headset_get_orientation(); + +// Deprecated functions + +// deprecated since Webots R2023a revision 1 +// use wb_supervisor_node_enable_contact_points_tracking or wb_supervisor_node_disable_contact_points_tracking instead +void wb_supervisor_node_enable_contact_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants); +void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants); + +// deprecated since Webots R2018b +void wb_supervisor_simulation_revert() WB_DEPRECATED; // please use wb_supervisor_world_reload() instead +void wb_supervisor_load_world(const char *filename) WB_DEPRECATED; // please use wb_supervisor_world_load() instead +bool wb_supervisor_save_world(const char *filename) WB_DEPRECATED; // please use wb_supervisor_world_save() instead + +// deprecated since Webots 8.6.0, plesae use wb_supervisor_field_remove_mf_item() instead +void wb_supervisor_field_remove_mf_node(WbFieldRef field, int position) WB_DEPRECATED; + +// deprecated since Webots 8.0.0, plesae use wb_supervisor_simulation_reset_physics() instead +void wb_supervisor_simulation_physics_reset() WB_DEPRECATED; + +// deprecated since Webots 8.4.0 please use wb_supervisor_movie_is_ready and wb_supervisor_movie_failed +#define WB_SUPERVISOR_MOVIE_READY 0 +#define WB_SUPERVISOR_MOVIE_RECORDING 1 +#define WB_SUPERVISOR_MOVIE_SAVING 2 +#define WB_SUPERVISOR_MOVIE_WRITE_ERROR 3 +#define WB_SUPERVISOR_MOVIE_ENCODING_ERROR 4 +#define WB_SUPERVISOR_MOVIE_SIMULATION_ERROR 5 +int wb_supervisor_movie_get_status(); + +// deprecated since webots 8.3.0: please use the wb_supervisor_movie_*() functions instead +void wb_supervisor_start_movie(const char *file, int width, int height, int codec, int quality, int acceleration, + bool caption) WB_DEPRECATED; +void wb_supervisor_stop_movie() WB_DEPRECATED; +int wb_supervisor_get_movie_status() WB_DEPRECATED; + +#ifdef __cplusplus +} +#endif + +#endif /* SUPERVISOR_H */ diff --git a/include/controller/c/webots/touch_sensor.h b/include/controller/c/webots/touch_sensor.h new file mode 100644 index 0000000..5f9a371 --- /dev/null +++ b/include/controller/c/webots/touch_sensor.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the TouchSensor node */ +/**********************************************************************************/ + +#ifndef WB_TOUCH_SENSOR_H +#define WB_TOUCH_SENSOR_H + +#define WB_USING_C_API + +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_touch_sensor_enable(WbDeviceTag tag, int sampling_period); +void wb_touch_sensor_disable(WbDeviceTag tag); +int wb_touch_sensor_get_sampling_period(WbDeviceTag tag); + +int wb_touch_sensor_get_lookup_table_size(WbDeviceTag tag); +const double *wb_touch_sensor_get_lookup_table(WbDeviceTag tag); + +double wb_touch_sensor_get_value(WbDeviceTag tag); +const double *wb_touch_sensor_get_values(WbDeviceTag tag); + +typedef enum { WB_TOUCH_SENSOR_BUMPER = 0, WB_TOUCH_SENSOR_FORCE, WB_TOUCH_SENSOR_FORCE3D } WbTouchSensorType; + +WbTouchSensorType wb_touch_sensor_get_type(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_TOUCH_SENSOR_H */ diff --git a/include/controller/c/webots/types.h b/include/controller/c/webots/types.h new file mode 100644 index 0000000..4334ea5 --- /dev/null +++ b/include/controller/c/webots/types.h @@ -0,0 +1,83 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Common definitions for both the C and C++ APIs */ +/**********************************************************************************/ + +#if defined(WB_USING_CPP_API) && defined(WB_USING_C_API) && !defined(WB_ALLOW_MIXING_C_AND_CPP_API) +#ifdef _MSC_VER +#pragma message("warning: mixing the C and C++ APIs in the same controller is not supported.") +#else +#warning "mixing the C and C++ APIs in the same controller is not supported." +#endif +#endif + +#ifndef WB_TYPES_H +#define WB_TYPES_H + +// There can be a maximum of 65534 devices on a robot (65535 being reserved) +typedef unsigned short WbDeviceTag; // identifier of a device + +// Opaque type definitions +typedef struct WbImageStructPrivate *WbImageRef; +typedef struct WbMotionStructPrivate *WbMotionRef; +typedef struct WbNodeStructPrivate *WbNodeRef; +typedef struct WbProtoInfoStructPrivate *WbProtoRef; +typedef struct WbFieldStructPrivate *WbFieldRef; + +// define "bool" type for C controllers +// C++ code will use the standard definition of "bool" +#ifndef __cplusplus + +#include // definition of INFINITY +#ifndef INFINITY +#define INFINITY (1.0 / 0.0) +#endif + +#ifndef bool +#define bool char +#endif + +#ifndef true +// clang-format off +#define true ((bool)1) +// clang-format on +#endif + +#ifndef false +// clang-format off +#define false ((bool)0) +// clang-format on +#endif + +#endif + +#define WB_ANGULAR 0 // kept for backward compatibility R2018b +typedef enum { WB_ROTATIONAL = 0, WB_LINEAR } WbJointType; + +// Allow us to mark functions as 'deprecated' and have gcc emit a nice warning for each use. +// Usage: int foo(char) WB_DEPRECATED; +// and then gcc will emit a warning for each usage of the function. +#ifndef WB_DEPRECATED +#if __GNUC__ >= 3 && !defined WB_MATLAB_LOADLIBRARY +#define WB_DEPRECATED __attribute__((deprecated)) +#else +#define WB_DEPRECATED +#endif +#endif + +#endif /* WB_TYPES_H */ diff --git a/include/controller/c/webots/utils/ansi_codes.h b/include/controller/c/webots/utils/ansi_codes.h new file mode 100644 index 0000000..8f9edda --- /dev/null +++ b/include/controller/c/webots/utils/ansi_codes.h @@ -0,0 +1,63 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef WBU_ANSI_CODES_H +#define WBU_ANSI_CODES_H + +#ifndef __cplusplus +#include +#endif + +#define ANSI_RESET "\x1b[0m" + +#define ANSI_BOLD "\x1b[1m" +#define ANSI_UNDERLINE "\x1b[4m" + +#define ANSI_BLACK_BACKGROUND "\x1b[40m" +#define ANSI_RED_BACKGROUND "\x1b[41m" +#define ANSI_GREEN_BACKGROUND "\x1b[42m" +#define ANSI_YELLOW_BACKGROUND "\x1b[43m" +#define ANSI_BLUE_BACKGROUND "\x1b[44m" +#define ANSI_MAGENTA_BACKGROUND "\x1b[45m" +#define ANSI_CYAN_BACKGROUND "\x1b[46m" +#define ANSI_WHITE_BACKGROUND "\x1b[47m" + +#define ANSI_BLACK_FOREGROUND "\x1b[30m" +#define ANSI_RED_FOREGROUND "\x1b[31m" +#define ANSI_GREEN_FOREGROUND "\x1b[32m" +#define ANSI_YELLOW_FOREGROUND "\x1b[33m" +#define ANSI_BLUE_FOREGROUND "\x1b[34m" +#define ANSI_MAGENTA_FOREGROUND "\x1b[35m" +#define ANSI_CYAN_FOREGROUND "\x1b[36m" +#define ANSI_WHITE_FOREGROUND "\x1b[37m" + +#define ANSI_CLEAR_SCREEN "\x1b[2J" + +#ifndef __cplusplus +// Convenient macros - Used only in C +#define ANSI_PRINTF_IN_BLACK(x, ...) printf(ANSI_BLACK_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_RED(x, ...) printf(ANSI_RED_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_GREEN(x, ...) printf(ANSI_GREEN_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_YELLOW(x, ...) printf(ANSI_YELLOW_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_BLUE(x, ...) printf(ANSI_BLUE_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_MAGENTA(x, ...) printf(ANSI_MAGENTA_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_CYAN(x, ...) printf(ANSI_CYAN_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) +#define ANSI_PRINTF_IN_WHITE(x, ...) printf(ANSI_WHITE_FOREGROUND x ANSI_RESET, ##__VA_ARGS__) + +#define ANSI_CLEAR_CONSOLE() printf(ANSI_CLEAR_SCREEN "\n") +#endif /* __cplusplus */ + +#endif /* WBU_ANSI_CODES_H */ diff --git a/include/controller/c/webots/utils/motion.h b/include/controller/c/webots/utils/motion.h new file mode 100644 index 0000000..585c977 --- /dev/null +++ b/include/controller/c/webots/utils/motion.h @@ -0,0 +1,47 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for Motion file playback */ +/**********************************************************************************/ + +#ifndef WBU_MOTION_H +#define WBU_MOTION_H + +#include "../types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +WbMotionRef wbu_motion_new(const char *filename); +void wbu_motion_delete(WbMotionRef motion); + +void wbu_motion_play(WbMotionRef motion); +void wbu_motion_stop(WbMotionRef motion); +void wbu_motion_set_loop(WbMotionRef motion, bool loop); +void wbu_motion_set_reverse(WbMotionRef motion, bool reverse); + +bool wbu_motion_is_over(WbMotionRef motion); +int wbu_motion_get_duration(WbMotionRef motion); +int wbu_motion_get_time(WbMotionRef motion); +void wbu_motion_set_time(WbMotionRef motion, int time); + +#ifdef __cplusplus +} +#endif + +#endif /* WBU_MOTION_H */ diff --git a/include/controller/c/webots/utils/string.h b/include/controller/c/webots/utils/string.h new file mode 100644 index 0000000..a7d9364 --- /dev/null +++ b/include/controller/c/webots/utils/string.h @@ -0,0 +1,35 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C utility to parse messages */ +/**********************************************************************************/ + +#ifndef WBU_STRING_H +#define WBU_STRING_H + +#ifdef __cplusplus +extern "C" { +#endif + +char *wbu_string_strsep(char **stringp, const char *delim); +char *wbu_string_replace(char *value, const char *before, const char *after); + +#ifdef __cplusplus +} +#endif + +#endif // WBU_STRING_H diff --git a/include/controller/c/webots/utils/system.h b/include/controller/c/webots/utils/system.h new file mode 100644 index 0000000..27cee79 --- /dev/null +++ b/include/controller/c/webots/utils/system.h @@ -0,0 +1,55 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for cross-platform system functions */ +/************************************************************************************/ + +#ifndef WBU_SYSTEM_H +#define WBU_SYSTEM_H + +#include "../types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// wbu_system_getenv() returns an UTF-8 multi-byte string for the specified +// environment variable. +// The return value points to a char buffer which may be overwritten by any subsequent +// call to a wbu_system function. +const char *wbu_system_getenv(const char *variable); + +// On Linux and macOS, wbu_system_short_path() returns the variable passed as an +// argument. On Windows, it returns an ASCII string corresponding to the Windows 8.3 +// short path (see GetShortPathName in the Windows API). This is useful when using a +// library that doesn't support UTF-8 multi-byte strings or wide characters for paths. +// The return value points to a char buffer which may be overwritten by any subsequent +// call to a wbu_system function. +const char *wbu_system_short_path(const char *path); + +// Return the system tmp folder used by Webots, typically /tmp on Linux +const char *wbu_system_tmpdir(); + +// The following function returns the folder used by the current instance of Webots. On Linux, it is /tmp/webots-XXX. +// On macOS is it /var/tmp/webots-XXX. On Windows, it is LOCALAPPDATA/Temp/webots-XXX where XXX is the Webots TCP port +const char *wbu_system_webots_instance_path(bool refresh); + +#ifdef __cplusplus +} +#endif + +#endif /* WBU_SYSTEM_H */ diff --git a/include/controller/c/webots/vacuum_gripper.h b/include/controller/c/webots/vacuum_gripper.h new file mode 100644 index 0000000..6b65d0f --- /dev/null +++ b/include/controller/c/webots/vacuum_gripper.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Webots C programming interface for the VacuumGripper node */ +/**********************************************************************************/ + +#ifndef WB_VACUUM_GRIPPER_H +#define WB_VACUUM_GRIPPER_H + +#define WB_USING_C_API +#include "types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void wb_vacuum_gripper_enable_presence(WbDeviceTag tag, int sampling_period); +void wb_vacuum_gripper_disable_presence(WbDeviceTag tag); +int wb_vacuum_gripper_get_presence_sampling_period(WbDeviceTag tag); +bool wb_vacuum_gripper_get_presence(WbDeviceTag tag); +void wb_vacuum_gripper_turn_on(WbDeviceTag tag); +void wb_vacuum_gripper_turn_off(WbDeviceTag tag); +bool wb_vacuum_gripper_is_on(WbDeviceTag tag); + +#ifdef __cplusplus +} +#endif + +#endif /* WB_VACUUM_GRIPPER_H */ diff --git a/include/controller/c/webots/vehicle/car.h b/include/controller/c/webots/vehicle/car.h new file mode 100644 index 0000000..6bf9f49 --- /dev/null +++ b/include/controller/c/webots/vehicle/car.h @@ -0,0 +1,87 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Car library to be used with the 'Car' proto (or any proto inherited by 'Car') + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#ifndef CAR_H +#define CAR_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { WBU_CAR_TRACTION = 0, WBU_CAR_PROPULSION = 1, WBU_CAR_FOUR_BY_FOUR = 2 } WbuCarType; + +typedef enum { + WBU_CAR_COMBUSTION_ENGINE = 0, + WBU_CAR_ELECTRIC_ENGINE = 1, + WBU_CAR_PARALLEL_HYBRID_ENGINE = 2, + WBU_CAR_POWER_SPLIT_HYBRID_ENGINE = 3 +} WbuCarEngineType; + +typedef enum { + WBU_CAR_WHEEL_FRONT_RIGHT = 0, + WBU_CAR_WHEEL_FRONT_LEFT = 1, + WBU_CAR_WHEEL_REAR_RIGHT = 2, + WBU_CAR_WHEEL_REAR_LEFT = 3, + WBU_CAR_WHEEL_NB +} WbuCarWheelIndex; + +void wbu_car_init(); +void wbu_car_cleanup(); + +WbuCarType wbu_car_get_type(); +WbuCarEngineType wbu_car_get_engine_type(); + +void wbu_car_set_indicator_period(double period); +double wbu_car_get_indicator_period(); + +bool wbu_car_get_backwards_lights(); +bool wbu_car_get_brake_lights(); + +double wbu_car_get_track_front(); +double wbu_car_get_track_rear(); +double wbu_car_get_wheelbase(); +double wbu_car_get_front_wheel_radius(); +double wbu_car_get_rear_wheel_radius(); + +double wbu_car_get_wheel_encoder(WbuCarWheelIndex wheel_index); +double wbu_car_get_wheel_speed(WbuCarWheelIndex wheel_index); + +void wbu_car_set_left_steering_angle(double angle); +void wbu_car_set_right_steering_angle(double angle); +double wbu_car_get_right_steering_angle(); +double wbu_car_get_left_steering_angle(); + +void wbu_car_enable_limited_slip_differential(bool enable); +void wbu_car_enable_indicator_auto_disabling(bool enable); + +// kept for backward compatibility (Webots R2018a) +typedef WbuCarType wbu_car_type; +typedef WbuCarEngineType wbu_car_engine_type; +typedef WbuCarWheelIndex wbu_car_wheel_index; + +#ifdef __cplusplus +} +#endif + +#endif // CAR_H diff --git a/include/controller/c/webots/vehicle/driver.h b/include/controller/c/webots/vehicle/driver.h new file mode 100644 index 0000000..91a492d --- /dev/null +++ b/include/controller/c/webots/vehicle/driver.h @@ -0,0 +1,97 @@ +/* + * Copyright 1996-2024 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 + * + * https://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: Driver library to be used with the 'Car' proto (or any proto inherited by 'Car') and the car library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#ifndef DRIVER_H +#define DRIVER_H + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { OFF, RIGHT, LEFT } WbuDriverIndicatorState; + +typedef enum { UNDEFINED_CONTROL_MODE = -1, SPEED = 0, TORQUE } WbuDriverControlMode; + +typedef enum { DOWN, SLOW, NORMAL, FAST } WbuDriverWiperMode; + +// private function for webots_ros2 to identify robots that can use libdriver +bool wbu_driver_initialization_is_possible(); + +void wbu_driver_init(); +void wbu_driver_cleanup(); +int wbu_driver_step(); + +// positive: turn right, negative: turn left +void wbu_driver_set_steering_angle(double steering_angle); +double wbu_driver_get_steering_angle(); + +void wbu_driver_set_cruising_speed(double speed); +double wbu_driver_get_target_cruising_speed(); + +double wbu_driver_get_current_speed(); + +void wbu_driver_set_throttle(double throttle); +double wbu_driver_get_throttle(); + +void wbu_driver_set_brake_intensity(double intensity); +double wbu_driver_get_brake_intensity(); + +void wbu_driver_set_indicator(WbuDriverIndicatorState state); +void wbu_driver_set_hazard_flashers(bool state); + +WbuDriverIndicatorState wbu_driver_get_indicator(); +bool wbu_driver_get_hazard_flashers(); + +void wbu_driver_set_dipped_beams(bool state); +void wbu_driver_set_antifog_lights(bool state); + +bool wbu_driver_get_dipped_beams(); +bool wbu_driver_get_antifog_lights(); + +double wbu_driver_get_rpm(); +int wbu_driver_get_gear(); +void wbu_driver_set_gear(int gear); +int wbu_driver_get_gear_number(); +WbuDriverControlMode wbu_driver_get_control_mode(); + +void wbu_driver_set_wiper_mode(WbuDriverWiperMode mode); +WbuDriverWiperMode wbu_driver_get_wiper_mode(); + +// kept for backward compatibility (Webots 8.6) +void wbu_driver_set_brake(double brake) WB_DEPRECATED; +double wbu_driver_get_brake() WB_DEPRECATED; + +// kept for backward compatibility (Webots R2018a) +typedef WbuDriverWiperMode wbu_wipers_mode; +typedef WbuDriverIndicatorState wbu_indicator_state; +typedef WbuDriverControlMode wbu_control_mode; +typedef WbuDriverWiperMode wbu_wipers_mode; +void wbu_driver_set_wipers_mode(wbu_wipers_mode mode) WB_DEPRECATED; +wbu_wipers_mode wbu_driver_get_wipers_mode() WB_DEPRECATED; + +#ifdef __cplusplus +} +#endif + +#endif // DRIVER_H diff --git a/include/controller/cpp/webots/Accelerometer.hpp b/include/controller/cpp/webots/Accelerometer.hpp new file mode 100644 index 0000000..e4f832c --- /dev/null +++ b/include/controller/cpp/webots/Accelerometer.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef ACCELEROMETER_HPP +#define ACCELEROMETER_HPP + +#include + +namespace webots { + class Accelerometer : public Device { + public: + explicit Accelerometer(const std::string &name) : Device(name) {} // Use Robot::getAccelerometer() instead + explicit Accelerometer(WbDeviceTag tag) : Device(tag) {} + virtual ~Accelerometer() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const double *getValues() const; + int getLookupTableSize() const; + const double *getLookupTable() const; + }; +} // namespace webots + +#endif // ACCELEROMETER_HPP diff --git a/include/controller/cpp/webots/Altimeter.hpp b/include/controller/cpp/webots/Altimeter.hpp new file mode 100644 index 0000000..6440464 --- /dev/null +++ b/include/controller/cpp/webots/Altimeter.hpp @@ -0,0 +1,33 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef ALTIMETER_HPP +#define ALTIMETER_HPP + +#include + +namespace webots { + class Altimeter : public Device { + public: + explicit Altimeter(const std::string &name) : Device(name) {} // Use Robot::getAltimeter instead + explicit Altimeter(WbDeviceTag tag) : Device(tag) {} + virtual ~Altimeter() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + double getValue() const; + }; +} // namespace webots + +#endif // ALTIMETER_HPP diff --git a/include/controller/cpp/webots/Brake.hpp b/include/controller/cpp/webots/Brake.hpp new file mode 100644 index 0000000..240d735 --- /dev/null +++ b/include/controller/cpp/webots/Brake.hpp @@ -0,0 +1,55 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef BRAKE_HPP +#define BRAKE_HPP + +#include +#include + +namespace webots { + class Motor; + class PositionSensor; + + class Brake : public Device { + public: + typedef enum { ROTATIONAL = 0, LINEAR } Type; + + explicit Brake(const std::string &name) : + Device(name), + motor(NULL), + positionSensor(NULL) {} // Use Robot::getBrake() instead + explicit Brake(WbDeviceTag tag) : Device(tag), motor(NULL), positionSensor(NULL) {} + virtual ~Brake() {} + Type getType() const; + void setDampingConstant(double dampingConstant) const; + + Motor *getMotor(); + PositionSensor *getPositionSensor(); + + // internal functions + int getMotorTag() const; + int getPositionSensorTag() const; + + enum { // kept for backward compatibility R2018b + ANGULAR = 0 + }; + + private: + Motor *motor; + PositionSensor *positionSensor; + }; +} // namespace webots + +#endif // BRAKE_HPP diff --git a/include/controller/cpp/webots/Camera.hpp b/include/controller/cpp/webots/Camera.hpp new file mode 100644 index 0000000..083b287 --- /dev/null +++ b/include/controller/cpp/webots/Camera.hpp @@ -0,0 +1,68 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef CAMERA_HPP +#define CAMERA_HPP + +#include +#include "../../c/webots/camera_recognition_object.h" + +namespace webots { + typedef WbCameraRecognitionObject CameraRecognitionObject; + + class Camera : public Device { + public: + explicit Camera(const std::string &name) : Device(name) {} // Use Robot::getCamera() instead + explicit Camera(WbDeviceTag tag) : Device(tag) {} + virtual ~Camera() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const unsigned char *getImage() const; + int getWidth() const; + int getHeight() const; + double getFov() const; + double getMaxFov() const; + double getMinFov() const; + virtual void setFov(double fov); + double getExposure() const; + void setExposure(double exposure); + double getFocalLength() const; + double getFocalDistance() const; + double getMaxFocalDistance() const; + double getMinFocalDistance() const; + virtual void setFocalDistance(double focalDistance); + double getNear() const; + int saveImage(const std::string &filename, int quality) const; + bool hasRecognition() const; + void recognitionEnable(int samplingPeriod); + void recognitionDisable(); + int getRecognitionSamplingPeriod() const; + int getRecognitionNumberOfObjects() const; + const CameraRecognitionObject *getRecognitionObjects() const; + bool hasRecognitionSegmentation() const; + void enableRecognitionSegmentation(); + void disableRecognitionSegmentation(); + bool isRecognitionSegmentationEnabled() const; + const unsigned char *getRecognitionSegmentationImage() const; + int saveRecognitionSegmentationImage(const std::string &filename, int quality) const; + static unsigned char imageGetRed(const unsigned char *image, int width, int x, int y); + static unsigned char imageGetGreen(const unsigned char *image, int width, int x, int y); + static unsigned char imageGetBlue(const unsigned char *image, int width, int x, int y); + static unsigned char imageGetGray(const unsigned char *image, int width, int x, int y); + // alias + static unsigned char imageGetGrey(const unsigned char *image, int width, int x, int y); + }; +} // namespace webots +#endif // CAMERA_HPP diff --git a/include/controller/cpp/webots/Compass.hpp b/include/controller/cpp/webots/Compass.hpp new file mode 100644 index 0000000..76df59e --- /dev/null +++ b/include/controller/cpp/webots/Compass.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef COMPASS_HPP +#define COMPASS_HPP + +#include + +namespace webots { + class Compass : public Device { + public: + explicit Compass(const std::string &name) : Device(name) {} // Use Robot::getCompass() instead + explicit Compass(WbDeviceTag tag) : Device(tag) {} + virtual ~Compass() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const double *getValues() const; + int getLookupTableSize() const; + const double *getLookupTable() const; + }; +} // namespace webots + +#endif // COMPASS_HPP diff --git a/include/controller/cpp/webots/Connector.hpp b/include/controller/cpp/webots/Connector.hpp new file mode 100644 index 0000000..6cd5030 --- /dev/null +++ b/include/controller/cpp/webots/Connector.hpp @@ -0,0 +1,36 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef CONNECTOR_HPP +#define CONNECTOR_HPP + +#include + +namespace webots { + class Connector : public Device { + public: + explicit Connector(const std::string &name) : Device(name) {} // Use Robot::getConnector() instead + explicit Connector(WbDeviceTag tag) : Device(tag) {} + virtual ~Connector() {} + virtual void enablePresence(int samplingPeriod); + virtual void disablePresence(); + int getPresenceSamplingPeriod() const; + int getPresence() const; + bool isLocked() const; + virtual void lock(); + virtual void unlock(); + }; +} // namespace webots + +#endif // CONNECTOR_HPP diff --git a/include/controller/cpp/webots/Device.hpp b/include/controller/cpp/webots/Device.hpp new file mode 100644 index 0000000..bb012f1 --- /dev/null +++ b/include/controller/cpp/webots/Device.hpp @@ -0,0 +1,43 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef DEVICE_HPP +#define DEVICE_HPP + +#define WB_USING_CPP_API +#include +#include "../../c/webots/types.h" + +namespace webots { + class Device { + public: + virtual ~Device() {} + const std::string &getName() const { return name; } + std::string getModel() const; + int getNodeType() const; + int getTag() const { return tag; } + + static bool hasType(int tag, int type); + + protected: + explicit Device(const std::string &name); + explicit Device(WbDeviceTag tag); + + private: + WbDeviceTag tag; + std::string name; + }; +} // namespace webots + +#endif // DEVICE_HPP diff --git a/include/controller/cpp/webots/Display.hpp b/include/controller/cpp/webots/Display.hpp new file mode 100644 index 0000000..988a943 --- /dev/null +++ b/include/controller/cpp/webots/Display.hpp @@ -0,0 +1,54 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef DISPLAY_HPP +#define DISPLAY_HPP + +#include +#include + +namespace webots { + class Camera; + class Display : public Device { + public: + enum { RGB = 3, RGBA, ARGB, BGRA, ABGR }; + explicit Display(const std::string &name) : Device(name) {} // Use Robot::getDisplay() instead + explicit Display(WbDeviceTag tag) : Device(tag) {} + virtual ~Display() {} + int getWidth() const; + int getHeight() const; + virtual void setColor(int color); + virtual void setAlpha(double alpha); + virtual void setOpacity(double opacity); + virtual void setFont(const std::string &font, int size, bool antiAliasing); + virtual void attachCamera(Camera *camera); + virtual void detachCamera(); + virtual void drawPixel(int x1, int y1); + virtual void drawLine(int x1, int y1, int x2, int y2); + virtual void drawRectangle(int x, int y, int width, int height); + virtual void drawOval(int cx, int cy, int a, int b); + virtual void drawPolygon(const int *x, const int *y, int size); + virtual void drawText(const std::string &txt, int x, int y); + virtual void fillRectangle(int x, int y, int width, int height); + virtual void fillOval(int cx, int cy, int a, int b); + virtual void fillPolygon(const int *x, const int *y, int size); + ImageRef *imageNew(int width, int height, const void *data, int format) const; + ImageRef *imageCopy(int x, int y, int width, int height) const; + virtual void imagePaste(ImageRef *ir, int x, int y, bool blend = false); + ImageRef *imageLoad(const std::string &filename) const; + void imageSave(ImageRef *ir, const std::string &filename) const; + void imageDelete(ImageRef *ir) const; + }; +} // namespace webots +#endif // DISPLAY_HPP diff --git a/include/controller/cpp/webots/DistanceSensor.hpp b/include/controller/cpp/webots/DistanceSensor.hpp new file mode 100644 index 0000000..08ba1c2 --- /dev/null +++ b/include/controller/cpp/webots/DistanceSensor.hpp @@ -0,0 +1,41 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef DISTANCESENSOR_HPP +#define DISTANCESENSOR_HPP + +#include + +namespace webots { + class DistanceSensor : public Device { + public: + typedef enum { GENERIC = 0, INFRA_RED, SONAR, LASER } Type; + + explicit DistanceSensor(const std::string &name) : Device(name) {} // Use Robot::getDistanceSensor() instead + explicit DistanceSensor(WbDeviceTag tag) : Device(tag) {} + virtual ~DistanceSensor() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + double getValue() const; + double getMaxValue() const; + double getMinValue() const; + double getAperture() const; + int getLookupTableSize() const; + const double *getLookupTable() const; + Type getType() const; + }; +} // namespace webots + +#endif // DISTANCESENSOR_HPP diff --git a/include/controller/cpp/webots/Emitter.hpp b/include/controller/cpp/webots/Emitter.hpp new file mode 100644 index 0000000..649583b --- /dev/null +++ b/include/controller/cpp/webots/Emitter.hpp @@ -0,0 +1,36 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef EMITTER_HPP +#define EMITTER_HPP + +#include + +namespace webots { + class Emitter : public Device { + public: + explicit Emitter(const std::string &name) : Device(name) {} // Use Robot::getEmitter() instead + explicit Emitter(WbDeviceTag tag) : Device(tag) {} + virtual ~Emitter() {} + enum { CHANNEL_BROADCAST = -1 }; + virtual int send(const void *data, int size); + int getBufferSize() const; + virtual void setChannel(int channel); + int getChannel() const; + double getRange() const; + virtual void setRange(double range); + }; +} // namespace webots + +#endif // EMITTER_HPP diff --git a/include/controller/cpp/webots/Field.hpp b/include/controller/cpp/webots/Field.hpp new file mode 100644 index 0000000..8ad240c --- /dev/null +++ b/include/controller/cpp/webots/Field.hpp @@ -0,0 +1,128 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef FIELD_HPP +#define FIELD_HPP + +#define WB_USING_CPP_API +#include +#include +#include "../../c/webots/types.h" + +#ifdef MF_STRING +#undef MF_STRING +#endif + +namespace webots { + class Node; + class Field { + public: + typedef enum { + NO_FIELD = 0x00, + SF_BOOL = 0x01, + SF_INT32, + SF_FLOAT, + SF_VEC2F, + SF_VEC3F, + SF_ROTATION, + SF_COLOR, + SF_STRING, + SF_NODE, + MF = 0x10, + MF_BOOL, + MF_INT32, + MF_FLOAT, + MF_VEC2F, + MF_VEC3F, + MF_ROTATION, + MF_COLOR, + MF_STRING, + MF_NODE + } Type; + + std::string getName() const; + Type getType() const; + std::string getTypeName() const; + int getCount() const; + Field *getActualField() const; + + void enableSFTracking(int samplingPeriod); + void disableSFTracking(); + + bool getSFBool() const; + int getSFInt32() const; + double getSFFloat() const; + const double *getSFVec2f() const; + const double *getSFVec3f() const; + const double *getSFRotation() const; + const double *getSFColor() const; + std::string getSFString() const; + Node *getSFNode() const; + + bool getMFBool(int index) const; + int getMFInt32(int index) const; + double getMFFloat(int index) const; + const double *getMFVec2f(int index) const; + const double *getMFVec3f(int index) const; + const double *getMFRotation(int index) const; + const double *getMFColor(int index) const; + std::string getMFString(int index) const; + Node *getMFNode(int index) const; + + void setSFBool(bool value); + void setSFInt32(int value); + void setSFFloat(double value); + void setSFVec2f(const double values[2]); + void setSFVec3f(const double values[3]); + void setSFRotation(const double values[4]); + void setSFColor(const double values[3]); + void setSFString(const std::string &value); + + void setMFBool(int index, bool value); + void setMFInt32(int index, int value); + void setMFFloat(int index, double value); + void setMFVec2f(int index, const double values[2]); + void setMFVec3f(int index, const double values[3]); + void setMFRotation(int index, const double values[4]); + void setMFColor(int index, const double values[3]); + void setMFString(int index, const std::string &value); + + void insertMFBool(int index, bool value); + void insertMFInt32(int index, int value); + void insertMFFloat(int index, double value); + void insertMFVec2f(int index, const double values[2]); + void insertMFVec3f(int index, const double values[3]); + void insertMFRotation(int index, const double values[4]); + void insertMFColor(int index, const double values[3]); + void insertMFString(int index, const std::string &value); + + void removeMF(int index); + void removeSF(); + + void importMFNodeFromString(int position, const std::string &nodeString); + void importSFNodeFromString(const std::string &nodeString); + + // DO NOT USE THESE FUNCTIONS: THEY ARE RESERVED FOR INTERNAL USE: + static Field *findField(WbFieldRef ref); + static void cleanup(); + + private: + Field(WbFieldRef ref); + ~Field() {} + + WbFieldRef fieldRef; + }; +} // namespace webots + +#endif // FIELD_HPP diff --git a/include/controller/cpp/webots/GPS.hpp b/include/controller/cpp/webots/GPS.hpp new file mode 100644 index 0000000..2a13d40 --- /dev/null +++ b/include/controller/cpp/webots/GPS.hpp @@ -0,0 +1,43 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef GPS_HPP +#define GPS_HPP + +#include + +namespace webots { + class GPS : public Device { + public: + typedef enum { LOCAL = 0, WGS84 } CoordinateSystem; + + explicit GPS(const std::string &name) : Device(name) {} // Use Robot::getGPS() instead + explicit GPS(WbDeviceTag tag) : Device(tag) {} + virtual ~GPS() {} + + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + + const double *getValues() const; + double getSpeed() const; + const double *getSpeedVector() const; + + const CoordinateSystem getCoordinateSystem() const; + + static std::string convertToDegreesMinutesSeconds(double decimalDegree); + }; +} // namespace webots + +#endif // GPS_HPP diff --git a/include/controller/cpp/webots/Gyro.hpp b/include/controller/cpp/webots/Gyro.hpp new file mode 100644 index 0000000..1f52ab6 --- /dev/null +++ b/include/controller/cpp/webots/Gyro.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef GYRO_HPP +#define GYRO_HPP + +#include + +namespace webots { + class Gyro : public Device { + public: + explicit Gyro(const std::string &name) : Device(name) {} // Use Robot::getGyro() instead + explicit Gyro(WbDeviceTag tag) : Device(tag) {} + virtual ~Gyro() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const double *getValues() const; + int getLookupTableSize() const; + const double *getLookupTable() const; + }; +} // namespace webots + +#endif // GYRO_HPP diff --git a/include/controller/cpp/webots/ImageRef.hpp b/include/controller/cpp/webots/ImageRef.hpp new file mode 100644 index 0000000..82060ae --- /dev/null +++ b/include/controller/cpp/webots/ImageRef.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef IMAGE_REF_HPP +#define IMAGE_REF_HPP + +#define WB_USING_CPP_API +#include "../../c/webots/types.h" + +namespace webots { + class ImageRef { + public: + // Use Display::imageNew(), Display::imageCopy() or Display::imageLoad() instead + explicit ImageRef(const WbImageRef &imageRef) : imageRef(imageRef) {} + // Use Display::imageDelete() instead + virtual ~ImageRef() {} + WbImageRef getImageRef() { return imageRef; } + + private: + WbImageRef imageRef; + }; +} // namespace webots +#endif // IMAGE_REF_HPP diff --git a/include/controller/cpp/webots/InertialUnit.hpp b/include/controller/cpp/webots/InertialUnit.hpp new file mode 100644 index 0000000..e6065a6 --- /dev/null +++ b/include/controller/cpp/webots/InertialUnit.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef INERTIAL_UNIT_HPP +#define INERTIAL_UNIT_HPP + +#include + +namespace webots { + class InertialUnit : public Device { + public: + explicit InertialUnit(const std::string &name) : Device(name) {} // Use Robot::getInertialUnit() instead + explicit InertialUnit(WbDeviceTag tag) : Device(tag) {} + virtual ~InertialUnit() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const double *getRollPitchYaw() const; + const double *getQuaternion() const; + double getNoise() const; + }; +} // namespace webots + +#endif // INERTIAL_UNIT_HPP diff --git a/include/controller/cpp/webots/Joystick.hpp b/include/controller/cpp/webots/Joystick.hpp new file mode 100644 index 0000000..fb306c3 --- /dev/null +++ b/include/controller/cpp/webots/Joystick.hpp @@ -0,0 +1,43 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef JOYSTICK_HPP +#define JOYSTICK_HPP + +#include + +namespace webots { + class Joystick { + public: + Joystick() {} // Use Robot::getJoystick() instead + virtual ~Joystick() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + bool isConnected() const; + std::string getModel() const; + int getNumberOfAxes() const; + int getAxisValue(int axis) const; + int getNumberOfPovs() const; + int getPovValue(int pov) const; + int getPressedButton() const; + void setConstantForce(int level); + void setConstantForceDuration(double duration); + void setAutoCenteringGain(double gain); + void setResistanceGain(double gain); + void setForceAxis(int axis); + }; +} // namespace webots + +#endif // JOYSTICK_HPP diff --git a/include/controller/cpp/webots/Keyboard.hpp b/include/controller/cpp/webots/Keyboard.hpp new file mode 100644 index 0000000..345c3ee --- /dev/null +++ b/include/controller/cpp/webots/Keyboard.hpp @@ -0,0 +1,51 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef KEYBOARD_HPP +#define KEYBOARD_HPP + +namespace webots { + class Keyboard { + public: + enum { + END = 312, + HOME, + LEFT, + UP, + RIGHT, + DOWN, + PAGEUP = 366, + PAGEDOWN, + NUMPAD_HOME = 375, + NUMPAD_LEFT, + NUMPAD_UP, + NUMPAD_RIGHT, + NUMPAD_DOWN, + NUMPAD_END = 382, + KEY = 0x0000ffff, + SHIFT = 0x00010000, + CONTROL = 0x00020000, + ALT = 0x00040000 + }; + + Keyboard() {} // Use Robot::getKeyboard() instead + virtual ~Keyboard() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + int getKey() const; + }; +} // namespace webots + +#endif // KEYBOARD_HPP diff --git a/include/controller/cpp/webots/LED.hpp b/include/controller/cpp/webots/LED.hpp new file mode 100644 index 0000000..9d183a6 --- /dev/null +++ b/include/controller/cpp/webots/LED.hpp @@ -0,0 +1,31 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef LED_HPP +#define LED_HPP + +#include + +namespace webots { + class LED : public Device { + public: + explicit LED(const std::string &name) : Device(name) {} // Use Robot::getLED() instead + explicit LED(WbDeviceTag tag) : Device(tag) {} + virtual ~LED() {} + virtual void set(int value); + int get() const; + }; +} // namespace webots + +#endif // LED_HPP diff --git a/include/controller/cpp/webots/Lidar.hpp b/include/controller/cpp/webots/Lidar.hpp new file mode 100644 index 0000000..6148ee1 --- /dev/null +++ b/include/controller/cpp/webots/Lidar.hpp @@ -0,0 +1,52 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef LIDAR_HPP +#define LIDAR_HPP + +#include +#include "../../c/webots/lidar_point.h" + +namespace webots { + typedef WbLidarPoint LidarPoint; + + class Lidar : public Device { + public: + explicit Lidar(const std::string &name) : Device(name) {} // Use Robot::getLidar() instead + explicit Lidar(WbDeviceTag tag) : Device(tag) {} + virtual ~Lidar() {} + virtual void enable(int samplingPeriod); + void enablePointCloud(); + virtual void disable(); + void disablePointCloud(); + int getSamplingPeriod() const; + bool isPointCloudEnabled() const; + const float *getRangeImage() const; + const float *getLayerRangeImage(int layer) const; + const LidarPoint *getPointCloud() const; + const LidarPoint *getLayerPointCloud(int layer) const; + int getNumberOfPoints() const; + int getHorizontalResolution() const; + int getNumberOfLayers() const; + double getMinFrequency() const; + double getMaxFrequency() const; + double getFrequency() const; + void setFrequency(double frequency); + double getFov() const; + double getVerticalFov() const; + double getMinRange() const; + double getMaxRange() const; + }; +} // namespace webots +#endif // LIDAR_HPP diff --git a/include/controller/cpp/webots/LightSensor.hpp b/include/controller/cpp/webots/LightSensor.hpp new file mode 100644 index 0000000..7118816 --- /dev/null +++ b/include/controller/cpp/webots/LightSensor.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef LIGHTSENSOR_HPP +#define LIGHTSENSOR_HPP + +#include + +namespace webots { + class LightSensor : public Device { + public: + explicit LightSensor(const std::string &name) : Device(name) {} // Use Robot::getLightSensor() instead + explicit LightSensor(WbDeviceTag tag) : Device(tag) {} + virtual ~LightSensor() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + double getValue() const; + int getLookupTableSize() const; + const double *getLookupTable() const; + }; +} // namespace webots + +#endif // LIGHTSENSOR_HPP diff --git a/include/controller/cpp/webots/Motor.hpp b/include/controller/cpp/webots/Motor.hpp new file mode 100644 index 0000000..bf218d9 --- /dev/null +++ b/include/controller/cpp/webots/Motor.hpp @@ -0,0 +1,84 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef MOTOR_HPP +#define MOTOR_HPP + +#include + +#include // for 'INFINITY' + +namespace webots { + class Brake; + class PositionSensor; + + class Motor : public Device { + public: + typedef enum { ROTATIONAL = 0, LINEAR } Type; + + explicit Motor(const std::string &name) : + Device(name), + brake(NULL), + positionSensor(NULL) {} // Use Robot::getMotor() instead + explicit Motor(WbDeviceTag tag) : Device(tag), brake(NULL), positionSensor(NULL) {} + virtual ~Motor() {} + + virtual void setPosition(double position); + virtual void setVelocity(double vel); + virtual void setAcceleration(double acceleration); + virtual void setAvailableForce(double availableForce); + virtual void setAvailableTorque(double availableTorque); + virtual void setControlPID(double p, double i, double d); + + double getTargetPosition() const; + double getMinPosition() const; + double getMaxPosition() const; + double getVelocity() const; + double getMaxVelocity() const; + double getAcceleration() const; + double getAvailableForce() const; + double getMaxForce() const; + double getAvailableTorque() const; + double getMaxTorque() const; + double getMultiplier() const; + + virtual void enableForceFeedback(int samplingPeriod); + virtual void disableForceFeedback(); + int getForceFeedbackSamplingPeriod() const; + double getForceFeedback() const; + + virtual void enableTorqueFeedback(int samplingPeriod); + virtual void disableTorqueFeedback(); + int getTorqueFeedbackSamplingPeriod() const; + double getTorqueFeedback() const; + + virtual void setForce(double force); + virtual void setTorque(double torque); + + Type getType() const; + + Brake *getBrake(); + PositionSensor *getPositionSensor(); + + // internal functions + int getBrakeTag() const; + int getPositionSensorTag() const; + + private: + Brake *brake; + PositionSensor *positionSensor; + }; +} // namespace webots + +#endif // MOTOR_HPP diff --git a/include/controller/cpp/webots/Mouse.hpp b/include/controller/cpp/webots/Mouse.hpp new file mode 100644 index 0000000..f1bccd2 --- /dev/null +++ b/include/controller/cpp/webots/Mouse.hpp @@ -0,0 +1,37 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef MOUSE_HPP +#define MOUSE_HPP + +#include "../../c/webots/mouse_state.h" + +namespace webots { + typedef WbMouseState MouseState; + + class Mouse { + public: + Mouse() {} // Use Robot::getMouse() instead + virtual ~Mouse() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + void enable3dPosition(); + void disable3dPosition(); + bool is3dPositionEnabled() const; + MouseState getState() const; + }; +} // namespace webots + +#endif // MOUSE_HPP diff --git a/include/controller/cpp/webots/Node.hpp b/include/controller/cpp/webots/Node.hpp new file mode 100644 index 0000000..1f84470 --- /dev/null +++ b/include/controller/cpp/webots/Node.hpp @@ -0,0 +1,193 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef NODE_HPP +#define NODE_HPP + +#define WB_USING_CPP_API +#include +#include +#include +#include "../../c/webots/contact_point.h" +#include "../../c/webots/types.h" + +// Note: should match with node.h + +namespace webots { + typedef WbContactPoint ContactPoint; + + class Field; + class Proto; + class Node { + public: + typedef enum { + NO_NODE, + // 3D rendering + APPEARANCE, + BACKGROUND, + BILLBOARD, + BOX, + CAD_SHAPE, + CAPSULE, + COLOR, + CONE, + COORDINATE, + CYLINDER, + DIRECTIONAL_LIGHT, + ELEVATION_GRID, + FOG, + GROUP, + IMAGE_TEXTURE, + INDEXED_FACE_SET, + INDEXED_LINE_SET, + MATERIAL, + MESH, + MUSCLE, + NORMAL, + PBR_APPEARANCE, + PLANE, + POINT_LIGHT, + POINT_SET, + POSE, + SHAPE, + SPHERE, + SPOT_LIGHT, + TEXTURE_COORDINATE, + TEXTURE_TRANSFORM, + TRANSFORM, + VIEWPOINT, + // robots + ROBOT, + // devices + ACCELEROMETER, + ALTIMETER, + BRAKE, + CAMERA, + COMPASS, + CONNECTOR, + DISPLAY, + DISTANCE_SENSOR, + EMITTER, + GPS, + GYRO, + INERTIAL_UNIT, + LED, + LIDAR, + LIGHT_SENSOR, + LINEAR_MOTOR, + PEN, + POSITION_SENSOR, + PROPELLER, + RADAR, + RANGE_FINDER, + RECEIVER, + ROTATIONAL_MOTOR, + SKIN, + SPEAKER, + TOUCH_SENSOR, + VACUUM_GRIPPER, + // misc + BALL_JOINT, + BALL_JOINT_PARAMETERS, + CHARGER, + CONTACT_PROPERTIES, + DAMPING, + FLUID, + FOCUS, + HINGE_JOINT, + HINGE_JOINT_PARAMETERS, + HINGE_2_JOINT, + IMMERSION_PROPERTIES, + JOINT_PARAMETERS, + LENS, + LENS_FLARE, + PHYSICS, + RECOGNITION, + SLIDER_JOINT, + SLOT, + SOLID, + SOLID_REFERENCE, + TRACK, + TRACK_WHEEL, + WORLD_INFO, + ZOOM, + // experimental + MICROPHONE, + RADIO + } Type; + + virtual void remove(); + int getId() const; + Type getType() const; + std::string getDef() const; + std::string getTypeName() const; + std::string getBaseTypeName() const; + Node *getParentNode() const; + bool isProto() const; + Proto *getProto() const; + Node *getFromProtoDef(const std::string &name) const; + int getNumberOfFields() const; + int getNumberOfBaseNodeFields() const; + Field *getField(const std::string &fieldName) const; + Field *getBaseNodeField(const std::string &fieldName) const; + Field *getFieldByIndex(const int index) const; + Field *getBaseNodeFieldByIndex(const int index) const; + const double *getPosition() const; + const double *getOrientation() const; + const double *getPose() const; + const double *getPose(const Node *fromNode) const; + void enableContactPointsTracking(int samplingPeriod, bool includeDescendants = false) const; + void disableContactPointsTracking(bool includeDescendants = false) const; + void enablePoseTracking(int samplingPeriod) const; + void disablePoseTracking() const; + void enablePoseTracking(int samplingPeriod, const Node *fromNode) const; + void disablePoseTracking(const Node *fromNode) const; + ContactPoint *getContactPoints(bool includeDescendants, int *size) const; + const double *getCenterOfMass() const; + const double *getContactPoint(int index) const; + Node *getContactPointNode(int index) const; + int getNumberOfContactPoints(bool includeDescendants = false) const; + bool getStaticBalance() const; + const double *getVelocity() const; + std::string exportString() const; + + void setVelocity(const double velocity[6]); + void resetPhysics(); + void restartController(); + + void moveViewpoint() const; + void setVisibility(Node *from, bool visible); + + void addForce(const double force[3], bool relative); + void addForceWithOffset(const double force[3], const double offset[3], bool relative); + void addTorque(const double torque[3], bool relative); + + void saveState(const std::string &stateName); + void loadState(const std::string &stateName); + + void setJointPosition(double position, int index = 1); + + // DO NOT USE THESE FUNCTIONS: THEY ARE RESERVED FOR INTERNAL USE: + static Node *findNode(WbNodeRef ref); + static void cleanup(); + + private: + Node(WbNodeRef nodeRef); + virtual ~Node() {} + + WbNodeRef nodeRef; + }; +} // namespace webots + +#endif // NODE_HPP diff --git a/include/controller/cpp/webots/Pen.hpp b/include/controller/cpp/webots/Pen.hpp new file mode 100644 index 0000000..9827db8 --- /dev/null +++ b/include/controller/cpp/webots/Pen.hpp @@ -0,0 +1,31 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef PEN_HPP +#define PEN_HPP + +#include + +namespace webots { + class Pen : public Device { + public: + explicit Pen(const std::string &name) : Device(name) {} // Use Robot::getPen() instead + explicit Pen(WbDeviceTag tag) : Device(tag) {} + virtual ~Pen() {} + virtual void write(bool write); + virtual void setInkColor(int color, double density); + }; +} // namespace webots + +#endif // PEN_HPP diff --git a/include/controller/cpp/webots/PositionSensor.hpp b/include/controller/cpp/webots/PositionSensor.hpp new file mode 100644 index 0000000..1386e78 --- /dev/null +++ b/include/controller/cpp/webots/PositionSensor.hpp @@ -0,0 +1,57 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef POSITION_SENSOR_HPP +#define POSITION_SENSOR_HPP + +#include + +namespace webots { + class Brake; + class Motor; + + class PositionSensor : public Device { + public: + typedef enum { ROTATIONAL = 0, LINEAR } Type; + + explicit PositionSensor(const std::string &name) : + Device(name), + brake(NULL), + motor(NULL) {} // Use Robot::getPositionSensor() instead + explicit PositionSensor(WbDeviceTag tag) : Device(tag), brake(NULL), motor(NULL) {} + virtual ~PositionSensor() {} + virtual void enable(int samplingPeriod); // milliseconds + virtual void disable(); + int getSamplingPeriod() const; + double getValue() const; // rad or meters + Type getType() const; + + Brake *getBrake(); + Motor *getMotor(); + + // internal functions + int getBrakeTag() const; + int getMotorTag() const; + + enum { // kept for backward compatibility R2018b + ANGULAR = 0 + }; + + private: + Brake *brake; + Motor *motor; + }; +} // namespace webots + +#endif // POSITION_SENSOR_HPP diff --git a/include/controller/cpp/webots/Proto.hpp b/include/controller/cpp/webots/Proto.hpp new file mode 100644 index 0000000..4f1edb4 --- /dev/null +++ b/include/controller/cpp/webots/Proto.hpp @@ -0,0 +1,46 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef PROTO_HPP +#define PROTO_HPP + +#define WB_USING_CPP_API +#include +#include +#include "../../c/webots/types.h" + +namespace webots { + class Field; + class Proto { + public: + std::string getTypeName() const; + bool isDerived() const; + Proto *getParent() const; + Field *getField(const std::string &fieldName) const; + Field *getFieldByIndex(const int index) const; + int getNumberOfFields() const; + + // DO NOT USE THESE FUNCTIONS: THEY ARE RESERVED FOR INTERNAL USE: + static Proto *findProto(WbProtoRef ref); + static void cleanup(); + + private: + Proto(WbProtoRef ref); + ~Proto() {} + + WbProtoRef protoRef; + }; +} // namespace webots + +#endif // PROTO_HPP diff --git a/include/controller/cpp/webots/Radar.hpp b/include/controller/cpp/webots/Radar.hpp new file mode 100644 index 0000000..5de422d --- /dev/null +++ b/include/controller/cpp/webots/Radar.hpp @@ -0,0 +1,41 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef RADAR_HPP +#define RADAR_HPP + +#include +#include "../../c/webots/radar_target.h" + +namespace webots { + typedef WbRadarTarget RadarTarget; + + class Radar : public Device { + public: + explicit Radar(const std::string &name) : Device(name) {} // Use Robot::getRadar() instead + explicit Radar(WbDeviceTag tag) : Device(tag) {} + virtual ~Radar() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + int getNumberOfTargets() const; + const RadarTarget *getTargets() const; + double getMinRange() const; + double getMaxRange() const; + double getHorizontalFov() const; + double getVerticalFov() const; + }; +} // namespace webots + +#endif // RADAR_HPP diff --git a/include/controller/cpp/webots/RangeFinder.hpp b/include/controller/cpp/webots/RangeFinder.hpp new file mode 100644 index 0000000..bc5a629 --- /dev/null +++ b/include/controller/cpp/webots/RangeFinder.hpp @@ -0,0 +1,39 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef RANGE_FINDER_HPP +#define RANGE_FINDER_HPP + +#include + +namespace webots { + class RangeFinder : public Device { + public: + explicit RangeFinder(const std::string &name) : Device(name) {} // Use Robot::getRangeFinder() instead + explicit RangeFinder(WbDeviceTag tag) : Device(tag) {} + virtual ~RangeFinder() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + const float *getRangeImage() const; + int getWidth() const; + int getHeight() const; + double getFov() const; + double getMinRange() const; + double getMaxRange() const; + int saveImage(const std::string &filename, int quality) const; + static float rangeImageGetDepth(const float *image, int width, int x, int y); + }; +} // namespace webots +#endif // RANGE_FINDER_HPP diff --git a/include/controller/cpp/webots/Receiver.hpp b/include/controller/cpp/webots/Receiver.hpp new file mode 100644 index 0000000..2526350 --- /dev/null +++ b/include/controller/cpp/webots/Receiver.hpp @@ -0,0 +1,41 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef RECEIVER_HPP +#define RECEIVER_HPP + +#include + +namespace webots { + class Receiver : public Device { + public: + explicit Receiver(const std::string &name) : Device(name) {} // Use Robot::getReceiver() instead + explicit Receiver(WbDeviceTag tag) : Device(tag) {} + virtual ~Receiver() {} + enum { CHANNEL_BROADCAST = -1 }; + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + virtual void setChannel(int channel); + int getChannel() const; + int getQueueLength() const; + virtual void nextPacket(); + int getDataSize() const; + const void *getData() const; + double getSignalStrength() const; + const double *getEmitterDirection() const; + }; +} // namespace webots + +#endif // RECEIVER_HPP diff --git a/include/controller/cpp/webots/Robot.hpp b/include/controller/cpp/webots/Robot.hpp new file mode 100644 index 0000000..67b6634 --- /dev/null +++ b/include/controller/cpp/webots/Robot.hpp @@ -0,0 +1,181 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef ROBOT_HPP +#define ROBOT_HPP + +#include +#include + +#include "../../c/webots/types.h" + +namespace webots { + class Accelerometer; + class Altimeter; + class Brake; + class Camera; + class Compass; + class Connector; + class DistanceSensor; + class Display; + class Device; + class Emitter; + class GPS; + class Gyro; + class InertialUnit; + class Joystick; + class Keyboard; + class LED; + class Lidar; + class LightSensor; + class Motor; + class Mouse; + class Pen; + class PositionSensor; + class Radar; + class RangeFinder; + class Receiver; + class Skin; + class Speaker; + class TouchSensor; + class VacuumGripper; + + class Robot { + public: + typedef enum { MODE_SIMULATION = 0, MODE_CROSS_COMPILATION, MODE_REMOTE_CONTROL } Mode; + + typedef enum { + EVENT_QUIT = -1, + EVENT_NO_EVENT = 0, + EVENT_MOUSE_CLICK = 1, + EVENT_MOUSE_MOVE = 2, + EVENT_KEYBOARD = 4, + EVENT_JOYSTICK_BUTTON = 8, + EVENT_JOYSTICK_AXIS = 16, + EVENT_JOYSTICK_POV = 32 + } UserInputEvent; + + Robot(); + 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; + double getTime() const; + std::string getModel() const; + std::string getCustomData() const; + void setCustomData(const std::string &data); + Mode getMode() const; + void setMode(Mode, const char *); + bool getSupervisor() const; + bool getSynchronization() const; + std::string getProjectPath() const; + std::string getWorldPath() const; + double getBasicTimeStep() const; + int getNumberOfDevices() const; + Device *getDeviceByIndex(int index); + Device *getDevice(const std::string &name); + + virtual void batterySensorEnable(int samplingPeriod); + virtual void batterySensorDisable(); + int batterySensorGetSamplingPeriod() const; + double batterySensorGetValue() const; + + Accelerometer *getAccelerometer(const std::string &name); + Altimeter *getAltimeter(const std::string &name); + Brake *getBrake(const std::string &name); + Camera *getCamera(const std::string &name); + Compass *getCompass(const std::string &name); + Connector *getConnector(const std::string &name); + Display *getDisplay(const std::string &name); + DistanceSensor *getDistanceSensor(const std::string &name); + Emitter *getEmitter(const std::string &name); + GPS *getGPS(const std::string &name); + Gyro *getGyro(const std::string &name); + InertialUnit *getInertialUnit(const std::string &name); + Joystick *getJoystick() { return mJoystick; } + Keyboard *getKeyboard() { return mKeyboard; } + LED *getLED(const std::string &name); + Lidar *getLidar(const std::string &name); + LightSensor *getLightSensor(const std::string &name); + Motor *getMotor(const std::string &name); + Mouse *getMouse() { return mMouse; } + Pen *getPen(const std::string &name); + PositionSensor *getPositionSensor(const std::string &name); + Radar *getRadar(const std::string &name); + RangeFinder *getRangeFinder(const std::string &name); + Receiver *getReceiver(const std::string &name); + Skin *getSkin(const std::string &name); + Speaker *getSpeaker(const std::string &name); + TouchSensor *getTouchSensor(const std::string &name); + VacuumGripper *getVacuumGripper(const std::string &name); + + void *windowCustomFunction(void *arg); + void wwiSend(const char *data, int size); + void wwiSendText(const std::string &text); + const char *wwiReceive(int *size); + std::string wwiReceiveText(); + + // deprecated since Webots 2018a + std::string getData() const; + void setData(const std::string &data); + + // internal functions + static Device *getDeviceFromTag(int tag); + static int getDeviceTypeFromTag(int tag); + static std::string getDeviceNameFromTag(int tag); + static int getDeviceTagFromIndex(int index); + static int getDeviceTagFromName(const std::string &name); + + protected: + static Robot *cInstance; + virtual Accelerometer *createAccelerometer(WbDeviceTag tag) const; + virtual Altimeter *createAltimeter(WbDeviceTag tag) const; + virtual Brake *createBrake(WbDeviceTag tag) const; + virtual Camera *createCamera(WbDeviceTag tag) const; + virtual Compass *createCompass(WbDeviceTag tag) const; + virtual Connector *createConnector(WbDeviceTag tag) const; + virtual Display *createDisplay(WbDeviceTag tag) const; + virtual DistanceSensor *createDistanceSensor(WbDeviceTag tag) const; + virtual Emitter *createEmitter(WbDeviceTag tag) const; + virtual GPS *createGPS(WbDeviceTag tag) const; + virtual Gyro *createGyro(WbDeviceTag tag) const; + virtual InertialUnit *createInertialUnit(WbDeviceTag tag) const; + virtual LED *createLED(WbDeviceTag tag) const; + virtual Lidar *createLidar(WbDeviceTag tag) const; + virtual LightSensor *createLightSensor(WbDeviceTag tag) const; + virtual Motor *createMotor(WbDeviceTag tag) const; + virtual Pen *createPen(WbDeviceTag tag) const; + virtual PositionSensor *createPositionSensor(WbDeviceTag tag) const; + virtual Radar *createRadar(WbDeviceTag tag) const; + virtual RangeFinder *createRangeFinder(WbDeviceTag tag) const; + virtual Receiver *createReceiver(WbDeviceTag tag) const; + virtual Skin *createSkin(WbDeviceTag tag) const; + virtual Speaker *createSpeaker(WbDeviceTag tag) const; + virtual TouchSensor *createTouchSensor(WbDeviceTag tag) const; + virtual VacuumGripper *createVacuumGripper(WbDeviceTag tag) const; + + private: + Keyboard *mKeyboard; + Joystick *mJoystick; + Mouse *mMouse; + Device *getOrCreateDevice(int tag); + static std::vector deviceList; + }; +} // namespace webots + +#endif // ROBOT_HPP diff --git a/include/controller/cpp/webots/Skin.hpp b/include/controller/cpp/webots/Skin.hpp new file mode 100644 index 0000000..33c19de --- /dev/null +++ b/include/controller/cpp/webots/Skin.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef SKIN_HPP +#define SKIN_HPP + +#include + +namespace webots { + class Skin : public Device { + public: + explicit Skin(const std::string &name) : Device(name) {} + explicit Skin(WbDeviceTag tag) : Device(tag) {} + virtual ~Skin() {} + void setBoneOrientation(int index, const double orientation[4], bool absolute); + void setBonePosition(int index, const double position[3], bool absolute); + int getBoneCount() const; + const std::string getBoneName(int index) const; + const double *getBoneOrientation(int index, bool absolute) const; + const double *getBonePosition(int index, bool absolute) const; + }; +} // namespace webots + +#endif // SKIN_HPP diff --git a/include/controller/cpp/webots/Speaker.hpp b/include/controller/cpp/webots/Speaker.hpp new file mode 100644 index 0000000..9c80754 --- /dev/null +++ b/include/controller/cpp/webots/Speaker.hpp @@ -0,0 +1,39 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef SPEAKER_HPP +#define SPEAKER_HPP + +#include + +namespace webots { + class Speaker : public Device { + public: + explicit Speaker(const std::string &name) : Device(name) {} // Use Robot::getSpeaker() instead + explicit Speaker(WbDeviceTag tag) : Device(tag) {} + virtual ~Speaker() {} + static void playSound(Speaker *left, Speaker *right, const std::string &sound, double volume, double pitch, double balance, + bool loop); + bool isSoundPlaying(const std::string &sound) const; + void stop(const std::string &sound); + bool setEngine(const std::string &engine); + bool setLanguage(const std::string &language); + std::string getEngine(); + std::string getLanguage(); + void speak(const std::string &text, double volume); + bool isSpeaking() const; + }; +} // namespace webots + +#endif // SPEAKER_HPP diff --git a/include/controller/cpp/webots/Supervisor.hpp b/include/controller/cpp/webots/Supervisor.hpp new file mode 100644 index 0000000..6f9bc6e --- /dev/null +++ b/include/controller/cpp/webots/Supervisor.hpp @@ -0,0 +1,90 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef SUPERVISOR_HPP +#define SUPERVISOR_HPP + +#include +#include +#include + +namespace webots { + class Supervisor : public Robot { + public: + typedef enum { SIMULATION_MODE_PAUSE = 0, SIMULATION_MODE_REAL_TIME, SIMULATION_MODE_FAST } SimulationMode; + + Supervisor() : Robot() {} + static Supervisor *getSupervisorInstance(); + virtual ~Supervisor(); + virtual void simulationQuit(int status); + virtual void simulationReset(); + virtual void simulationResetPhysics(); + + SimulationMode simulationGetMode() const; + virtual void simulationSetMode(SimulationMode mode); + + virtual void worldLoad(const std::string &file); + virtual void worldReload(); + virtual bool worldSave(); + virtual bool worldSave(const std::string &file); + + void exportImage(const std::string &file, int quality) const; + + virtual bool animationStartRecording(const std::string &file); + virtual bool animationStopRecording(); + + virtual void movieStartRecording(const std::string &file, int width, int height, int codec, int quality, int acceleration, + bool caption); + virtual void movieStopRecording(); + bool movieIsReady() const; + bool movieFailed() const; + + virtual void setLabel(int id, const std::string &label, double xpos, double ypos, double size, int color, + double transparency = 0, const std::string &font = "Arial"); + Node *getRoot() const; + Node *getSelf() const; + Node *getFromDef(const std::string &name) const; + Node *getFromId(int id) const; + Node *getFromDevice(const Device *device) const; + Node *getFromDeviceTag(int tag) const; + Node *getSelected() const; + + bool virtualRealityHeadsetIsUsed() const; + const double *virtualRealityHeadsetGetPosition() const; + const double *virtualRealityHeadsetGetOrientation() const; + + // Deprecated functions + + // Deprecated since Webots R2018b + virtual void simulationRevert(); // please use worldReload() instead + virtual void loadWorld(const std::string &file); // please use worldLoad() instead + virtual bool saveWorld(); // please use worldSave() instead + virtual bool saveWorld(const std::string &file); // please use worldSave() instead + + // Deprecated since Webots 8.0.0, please use simulationResetPhysics() instead + virtual void simulationPhysicsReset(); + + // deprecated since webots 8.3.0: please use the wb_supervisor_movie_*() functions instead + virtual void startMovie(const std::string &file, int width, int height, int codec, int quality, int acceleration, + bool caption); + virtual void stopMovie(); + int getMovieStatus(); + + // deprecated since Webots 8.4.0: please use movieIsReady() and movieFailed() + enum { MOVIE_READY = 0, MOVIE_RECORDING, MOVIE_SAVING, MOVIE_WRITE_ERROR, MOVIE_ENCODING_ERROR, MOVIE_SIMULATION_ERROR }; + int movieGetStatus() const; + }; +} // namespace webots + +#endif // SUPERVISOR_HPP diff --git a/include/controller/cpp/webots/TouchSensor.hpp b/include/controller/cpp/webots/TouchSensor.hpp new file mode 100644 index 0000000..7da099b --- /dev/null +++ b/include/controller/cpp/webots/TouchSensor.hpp @@ -0,0 +1,43 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef TOUCHSENSOR_HPP +#define TOUCHSENSOR_HPP + +#include + +namespace webots { + class TouchSensor : public Device { + public: + typedef enum { BUMPER = 0, FORCE, FORCE3D } Type; + + explicit TouchSensor(const std::string &name) : Device(name) {} // Use Robot::getTouchSensor() instead + explicit TouchSensor(WbDeviceTag tag) : Device(tag) {} + virtual ~TouchSensor() {} + + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + + double getValue() const; + const double *getValues() const; + + int getLookupTableSize() const; + const double *getLookupTable() const; + + Type getType() const; + }; +} // namespace webots + +#endif // TOUCHSENSOR_HPP diff --git a/include/controller/cpp/webots/VacuumGripper.hpp b/include/controller/cpp/webots/VacuumGripper.hpp new file mode 100644 index 0000000..1888312 --- /dev/null +++ b/include/controller/cpp/webots/VacuumGripper.hpp @@ -0,0 +1,36 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef VACUUM_GRIPPER_HPP +#define VACUUM_GRIPPER_HPP + +#include + +namespace webots { + class VacuumGripper : public Device { + public: + explicit VacuumGripper(const std::string &name) : Device(name) {} // Use Robot::getVacuumGripper() instead + explicit VacuumGripper(WbDeviceTag tag) : Device(tag) {} + virtual ~VacuumGripper() {} + virtual void enablePresence(int samplingPeriod); + virtual void disablePresence(); + int getPresenceSamplingPeriod() const; + bool getPresence() const; + bool isOn() const; + virtual void turnOn(); + virtual void turnOff(); + }; +} // namespace webots + +#endif // VACUUM_GRIPPER_HPP diff --git a/include/controller/cpp/webots/utils/AnsiCodes.hpp b/include/controller/cpp/webots/utils/AnsiCodes.hpp new file mode 100644 index 0000000..5572e55 --- /dev/null +++ b/include/controller/cpp/webots/utils/AnsiCodes.hpp @@ -0,0 +1,47 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef ANSICODES_HPP +#define ANSICODES_HPP + +#include + +namespace webots { + namespace AnsiCodes { + static const std::string RESET = "\x1b[0m"; + static const std::string BOLD = "\x1b[1m"; + static const std::string UNDERLINE = "\x1b[4m"; + + static const std::string BLACK_FOREGROUND = "\x1b[30m"; + static const std::string RED_FOREGROUND = "\x1b[31m"; + static const std::string GREEN_FOREGROUND = "\x1b[32m"; + static const std::string YELLOW_FOREGROUND = "\x1b[33m"; + static const std::string BLUE_FOREGROUND = "\x1b[34m"; + static const std::string MAGENTA_FOREGROUND = "\x1b[35m"; + static const std::string CYAN_FOREGROUND = "\x1b[36m"; + static const std::string WHITE_FOREGROUND = "\x1b[37m"; + + static const std::string BLACK_BACKGROUND = "\x1b[40m"; + static const std::string RED_BACKGROUND = "\x1b[41m"; + static const std::string GREEN_BACKGROUND = "\x1b[42m"; + static const std::string YELLOW_BACKGROUND = "\x1b[43m"; + static const std::string BLUE_BACKGROUND = "\x1b[44m"; + static const std::string MAGENTA_BACKGROUND = "\x1b[45m"; + static const std::string CYAN_BACKGROUND = "\x1b[46m"; + static const std::string WHITE_BACKGROUND = "\x1b[47m"; + + static const std::string CLEAR_SCREEN = "\x1b[2J"; + } // namespace AnsiCodes +} // namespace webots +#endif /* ANSICODES_HPP */ diff --git a/include/controller/cpp/webots/utils/Motion.hpp b/include/controller/cpp/webots/utils/Motion.hpp new file mode 100644 index 0000000..eba80d4 --- /dev/null +++ b/include/controller/cpp/webots/utils/Motion.hpp @@ -0,0 +1,45 @@ +// Copyright 1996-2024 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 +// +// https://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. + +#ifndef MOTION_HPP +#define MOTION_HPP + +#define WB_USING_CPP_API +#include +#include "../../../c/webots/types.h" + +namespace webots { + class Motion { + public: + Motion(const std::string &fileName); + bool isValid() const; + virtual ~Motion(); + virtual void play(); + virtual void stop(); + int getDuration() const; + int getTime() const; + virtual void setTime(int time); + virtual void setReverse(bool reverse); + virtual void setLoop(bool loop); + bool isOver() const; + + protected: + WbMotionRef getMotionRef() const { return motionRef; } + + private: + WbMotionRef motionRef; + }; +} // namespace webots + +#endif // MOTION_HPP diff --git a/include/controller/cpp/webots/vehicle/Car.hpp b/include/controller/cpp/webots/vehicle/Car.hpp new file mode 100644 index 0000000..03b86c2 --- /dev/null +++ b/include/controller/cpp/webots/vehicle/Car.hpp @@ -0,0 +1,65 @@ +// Copyright 1996-2024 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 +// +// https://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: CPP wrapper of the car library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#ifndef CAR_HPP +#define CAR_HPP + +#include + +namespace webots { + class Car : public Driver { + public: + typedef enum { TRACTION = 0, PROPULSION, FOUR_BY_FOUR } Type; + + typedef enum { COMBUSTION_ENGINE = 0, ELECTRIC_ENGINE, PARALLEL_HYBRID_ENGINE, POWER_SPLIT_HYBRID_ENGINE } EngineType; + + typedef enum { WHEEL_FRONT_RIGHT = 0, WHEEL_FRONT_LEFT, WHEEL_REAR_RIGHT, WHEEL_REAR_LEFT, WHEEL_NB } WheelIndex; + + Car() : Driver() {} + virtual ~Car() {} + + Type getType(); + EngineType getEngineType(); + + void setIndicatorPeriod(double period); + double getIndicatorPeriod(); + + bool getBackwardsLights(); + bool getBrakeLights(); + + double getTrackFront(); + double getTrackRear(); + double getWheelbase(); + double getFrontWheelRadius(); + double getRearWheelRadius(); + + double getWheelEncoder(WheelIndex wheel); + double getWheelSpeed(WheelIndex wheel); + void setLeftSteeringAngle(double angle); + void setRightSteeringAngle(double angle); + double getRightSteeringAngle(); + double getLeftSteeringAngle(); + + void enableLimitedSlipDifferential(bool enable); + void enableIndicatorAutoDisabling(bool enable); + }; +} // namespace webots + +#endif // CAR_HPP diff --git a/include/controller/cpp/webots/vehicle/Driver.hpp b/include/controller/cpp/webots/vehicle/Driver.hpp new file mode 100644 index 0000000..a33338e --- /dev/null +++ b/include/controller/cpp/webots/vehicle/Driver.hpp @@ -0,0 +1,92 @@ +// Copyright 1996-2024 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 +// +// https://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: CPP wrapper of the driver library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#ifndef DRIVER_HPP +#define DRIVER_HPP + +#include + +namespace webots { + class Driver : public Supervisor { + public: + typedef enum { INDICATOR_OFF, INDICATOR_RIGHT, INDICATOR_LEFT } IndicatorState; + + typedef enum { SPEED, TORQUE } ControlMode; + + typedef enum { DOWN, SLOW, NORMAL, FAST } WiperMode; + + // private function for webots_ros2 to identify robots that can use libdriver + static bool isInitialisationPossible(); + + Driver(); + static Driver *getDriverInstance(); + virtual ~Driver(); + + virtual int step(); + + // positive: turn right, negative: turn left + void setSteeringAngle(double steeringAngle); + double getSteeringAngle(); + + void setCruisingSpeed(double speed); + double getTargetCruisingSpeed(); + + double getCurrentSpeed(); + + void setThrottle(double throttle); + double getThrottle(); + + void setBrakeIntensity(double intensity); + double getBrakeIntensity(); + + void setIndicator(IndicatorState state); + void setHazardFlashers(bool state); + + IndicatorState getIndicator(); + bool getHazardFlashers(); + + void setDippedBeams(bool state); + void setAntifogLights(bool state); + + bool getDippedBeams(); + bool getAntifogLights(); + + double getRpm(); + int getGear(); + void setGear(int gear); + int getGearNumber(); + ControlMode getControlMode(); + + void setWiperMode(WiperMode mode); + WiperMode getWiperMode(); + + // kept for backward compatibility only + void setBrake(double brake); + + void setWipersMode(WiperMode mode); + WiperMode getWipersMode(); + + private: + virtual int step(int t) { return Supervisor::step(t); } + static Driver *dInstance; + }; +} // namespace webots + +#endif // DRIVER_HPP diff --git a/src/controller/c/Controller.def b/src/controller/c/Controller.def new file mode 100644 index 0000000..d7fc80b --- /dev/null +++ b/src/controller/c/Controller.def @@ -0,0 +1,643 @@ +EXPORTS +wb_ANSI_RESET +wb_ANSI_BOLD +wb_ANSI_UNDERLINE +wb_ANSI_BLACK_BACKGROUND +wb_ANSI_RED_BACKGROUND +wb_ANSI_GREEN_BACKGROUND +wb_ANSI_YELLOW_BACKGROUND +wb_ANSI_BLUE_BACKGROUND +wb_ANSI_MAGENTA_BACKGROUND +wb_ANSI_CYAN_BACKGROUND +wb_ANSI_WHITE_BACKGROUND +wb_ANSI_BLACK_FOREGROUND +wb_ANSI_RED_FOREGROUND +wb_ANSI_GREEN_FOREGROUND +wb_ANSI_YELLOW_FOREGROUND +wb_ANSI_BLUE_FOREGROUND +wb_ANSI_MAGENTA_FOREGROUND +wb_ANSI_CYAN_FOREGROUND +wb_ANSI_WHITE_FOREGROUND +wb_ANSI_CLEAR_SCREEN +wb_KEYBOARD_END +wb_KEYBOARD_HOME +wb_KEYBOARD_LEFT +wb_KEYBOARD_UP +wb_KEYBOARD_RIGHT +wb_KEYBOARD_DOWN +wb_KEYBOARD_PAGEUP +wb_KEYBOARD_PAGEDOWN +wb_KEYBOARD_NUMPAD_HOME +wb_KEYBOARD_NUMPAD_LEFT +wb_KEYBOARD_NUMPAD_UP +wb_KEYBOARD_NUMPAD_RIGHT +wb_KEYBOARD_NUMPAD_DOWN +wb_KEYBOARD_NUMPAD_END +wb_KEYBOARD_KEY +wb_KEYBOARD_SHIFT +wb_KEYBOARD_CONTROL +wb_KEYBOARD_ALT +wb_LINEAR +wb_MF +wb_MF_BOOL +wb_MF_INT32 +wb_MF_FLOAT +wb_MF_VEC2F +wb_MF_VEC3F +wb_MF_ROTATION +wb_MF_COLOR +wb_MF_STRING +wb_MF_NODE +wb_NODE_NO_NODE +wb_NODE_APPEARANCE +wb_NODE_BACKGROUND +wb_NODE_BILLBOARD +wb_NODE_BOX +wb_NODE_CAD_SHAPE +wb_NODE_CAPSULE +wb_NODE_COLOR +wb_NODE_CONE +wb_NODE_COORDINATE +wb_NODE_CYLINDER +wb_NODE_DIRECTIONAL_LIGHT +wb_NODE_ELEVATION_GRID +wb_NODE_FOG +wb_NODE_GROUP +wb_NODE_IMAGE_TEXTURE +wb_NODE_INDEXED_FACE_SET +wb_NODE_INDEXED_LINE_SET +wb_NODE_MATERIAL +wb_NODE_MESH +wb_NODE_MUSCLE +wb_NODE_NORMAL +wb_NODE_PBR_APPEARANCE +wb_NODE_PLANE +wb_NODE_POINT_LIGHT +wb_NODE_POINT_SET +wb_NODE_POSE +wb_NODE_SHAPE +wb_NODE_SPHERE +wb_NODE_SPOT_LIGHT +wb_NODE_TEXTURE_COORDINATE +wb_NODE_TEXTURE_TRANSFORM +wb_NODE_TRANSFORM +wb_NODE_VIEWPOINT +wb_NODE_ROBOT +wb_NODE_ACCELEROMETER +wb_NODE_ALTIMETER +wb_NODE_BRAKE +wb_NODE_CAMERA +wb_NODE_COMPASS +wb_NODE_CONNECTOR +wb_NODE_DISPLAY +wb_NODE_DISTANCE_SENSOR +wb_NODE_EMITTER +wb_NODE_GPS +wb_NODE_GYRO +wb_NODE_INERTIAL_UNIT +wb_NODE_LED +wb_NODE_LIDAR +wb_NODE_LIGHT_SENSOR +wb_NODE_LINEAR_MOTOR +wb_NODE_PEN +wb_NODE_POSITION_SENSOR +wb_NODE_PROPELLER +wb_NODE_RADAR +wb_NODE_RANGE_FINDER +wb_NODE_RECEIVER +wb_NODE_ROTATIONAL_MOTOR +wb_NODE_SKIN +wb_NODE_SPEAKER +wb_NODE_TOUCH_SENSOR +wb_NODE_VACUUM_GRIPPER +wb_NODE_BALL_JOINT +wb_NODE_BALL_JOINT_PARAMETERS +wb_NODE_CHARGER +wb_NODE_CONTACT_PROPERTIES +wb_NODE_DAMPING +wb_NODE_FLUID +wb_NODE_FOCUS +wb_NODE_HINGE_JOINT +wb_NODE_HINGE_JOINT_PARAMETERS +wb_NODE_HINGE_2_JOINT +wb_NODE_IMMERSION_PROPERTIES +wb_NODE_JOINT_PARAMETERS +wb_NODE_LENS +wb_NODE_LENS_FLARE +wb_NODE_PHYSICS +wb_NODE_RECOGNITION +wb_NODE_SLIDER_JOINT +wb_NODE_SLOT +wb_NODE_SOLID +wb_NODE_SOLID_REFERENCE +wb_NODE_TRACK +wb_NODE_TRACK_WHEEL +wb_NODE_WORLD_INFO +wb_NODE_ZOOM +wb_NODE_MICROPHONE +wb_NODE_RADIO +wb_ROTATIONAL +wb_SF_BOOL +wb_SF_INT32 +wb_SF_FLOAT +wb_SF_VEC2F +wb_SF_VEC3F +wb_SF_ROTATION +wb_SF_COLOR +wb_SF_STRING +wb_SF_NODE +wb_accelerometer_disable +wb_accelerometer_enable +wb_accelerometer_get_sampling_period +wb_accelerometer_get_values +wb_accelerometer_get_lookup_table_size +wb_accelerometer_get_lookup_table +wb_altimeter_disable +wb_altimeter_enable +wb_altimeter_get_sampling_period +wb_altimeter_get_value +wb_brake_get_motor +wb_brake_get_position_sensor +wb_brake_get_type +wb_brake_set_damping_constant +wb_camera_disable +wb_camera_enable +wb_camera_get_exposure +wb_camera_get_focal_distance +wb_camera_get_focal_length +wb_camera_get_fov +wb_camera_get_height +wb_camera_get_image +wb_camera_get_max_focal_distance +wb_camera_get_max_fov +wb_camera_get_min_focal_distance +wb_camera_get_min_fov +wb_camera_get_near +wb_camera_get_sampling_period +wb_camera_get_width +wb_camera_has_recognition +wb_camera_recognition_disable +wb_camera_recognition_disable_segmentation +wb_camera_recognition_enable +wb_camera_recognition_enable_segmentation +wb_camera_recognition_get_number_of_objects +wb_camera_recognition_get_objects +wb_camera_recognition_get_sampling_period +wb_camera_recognition_get_segmentation_image +wb_camera_recognition_has_segmentation +wb_camera_recognition_is_segmentation_enabled +wb_camera_recognition_save_segmentation_image +wb_camera_save_image +wb_camera_set_exposure +wb_camera_set_focal_distance +wb_camera_set_fov +wb_compass_disable +wb_compass_enable +wb_compass_get_sampling_period +wb_compass_get_values +wb_compass_get_lookup_table_size +wb_compass_get_lookup_table +wb_connector_disable_presence +wb_connector_enable_presence +wb_connector_get_presence +wb_connector_get_presence_sampling_period +wb_connector_is_locked +wb_connector_lock +wb_connector_unlock +wb_console_print +wb_device_get_model +wb_device_get_name +wb_device_get_type +wb_device_get_node_type +wb_display_attach_camera +wb_display_detach_camera +wb_display_draw_line +wb_display_draw_oval +wb_display_draw_pixel +wb_display_draw_polygon +wb_display_draw_rectangle +wb_display_draw_text +wb_display_fill_oval +wb_display_fill_polygon +wb_display_fill_rectangle +wb_display_get_height +wb_display_get_width +wb_display_image_copy +wb_display_image_delete +wb_display_image_load +wb_display_image_new +wb_display_image_paste +wb_display_image_save +wb_display_set_alpha +wb_display_set_color +wb_display_set_font +wb_display_set_opacity +wb_distance_sensor_disable +wb_distance_sensor_enable +wb_distance_sensor_get_aperture +wb_distance_sensor_get_max_value +wb_distance_sensor_get_min_value +wb_distance_sensor_get_sampling_period +wb_distance_sensor_get_type +wb_distance_sensor_get_value +wb_distance_sensor_get_lookup_table_size +wb_distance_sensor_get_lookup_table +wb_emitter_get_buffer_size +wb_emitter_get_channel +wb_emitter_get_range +wb_emitter_send +wb_emitter_set_channel +wb_emitter_set_range +wb_gps_disable +wb_gps_convert_to_degrees_minutes_seconds +wb_gps_enable +wb_gps_get_coordinate_system +wb_gps_get_sampling_period +wb_gps_get_speed +wb_gps_get_speed_vector +wb_gps_get_values +wb_gyro_disable +wb_gyro_enable +wb_gyro_get_sampling_period +wb_gyro_get_values +wb_gyro_get_lookup_table_size +wb_gyro_get_lookup_table +wb_inertial_unit_disable +wb_inertial_unit_enable +wb_inertial_unit_get_roll_pitch_yaw +wb_inertial_unit_get_quaternion +wb_inertial_unit_get_sampling_period +wb_inertial_unit_get_noise +wb_joystick_disable +wb_joystick_enable +wb_joystick_get_axis_value +wb_joystick_get_model +wb_joystick_get_number_of_axes +wb_joystick_get_number_of_povs +wb_joystick_get_pov_value +wb_joystick_get_pressed_button +wb_joystick_get_sampling_period +wb_joystick_is_connected +wb_joystick_set_auto_centering_gain +wb_joystick_set_constant_force +wb_joystick_set_constant_force_duration +wb_joystick_set_force_axis +wb_joystick_set_resistance_gain +wb_keyboard_disable +wb_keyboard_enable +wb_keyboard_get_key +wb_keyboard_get_sampling_period +wb_led_get +wb_led_set +wb_lidar_disable +wb_lidar_disable_point_cloud +wb_lidar_enable +wb_lidar_enable_point_cloud +wb_lidar_get_fov +wb_lidar_get_frequency +wb_lidar_get_horizontal_resolution +wb_lidar_get_layer_point_cloud +wb_lidar_get_layer_range_image +wb_lidar_get_max_frequency +wb_lidar_get_max_range +wb_lidar_get_min_frequency +wb_lidar_get_min_range +wb_lidar_get_number_of_layers +wb_lidar_get_number_of_points +wb_lidar_get_point_cloud +wb_lidar_get_range_image +wb_lidar_get_sampling_period +wb_lidar_get_vertical_fov +wb_lidar_is_point_cloud_enabled +wb_lidar_set_frequency +wb_light_sensor_disable +wb_light_sensor_enable +wb_light_sensor_get_sampling_period +wb_light_sensor_get_value +wb_light_sensor_get_lookup_table_size +wb_light_sensor_get_lookup_table +wb_microphone_enable +wb_microphone_get_sample_data +wb_microphone_get_sample_size +wb_microphone_get_sampling_period +wb_motor_disable_force_feedback +wb_motor_disable_torque_feedback +wb_motor_enable_force_feedback +wb_motor_enable_torque_feedback +wb_motor_get_acceleration +wb_motor_get_available_force +wb_motor_get_available_torque +wb_motor_get_force_feedback +wb_motor_get_force_feedback_sampling_period +wb_motor_get_max_force +wb_motor_get_max_torque +wb_motor_get_max_position +wb_motor_get_max_velocity +wb_motor_get_min_position +wb_motor_get_multiplier +wb_motor_get_target_position +wb_motor_get_torque_feedback +wb_motor_get_torque_feedback_sampling_period +wb_motor_get_type +wb_motor_get_velocity +wb_motor_get_brake +wb_motor_get_position_sensor +wb_motor_set_acceleration +wb_motor_set_available_force +wb_motor_set_available_torque +wb_motor_set_control_pid +wb_motor_set_force +wb_motor_set_position +wb_motor_set_torque +wb_motor_set_velocity +wb_mouse_disable +wb_mouse_disable_3d_position +wb_mouse_enable +wb_mouse_enable_3d_position +wb_mouse_get_sampling_period +wb_mouse_get_state +wb_mouse_is_3d_position_enabled +wb_node_get_name +wb_pen_set_ink_color +wb_pen_write +wb_position_sensor_enable +wb_position_sensor_disable +wb_position_sensor_get_value +wb_position_sensor_get_sampling_period +wb_position_sensor_get_type +wb_position_sensor_get_brake +wb_position_sensor_get_motor +wb_radar_disable +wb_radar_enable +wb_radar_get_horizontal_fov +wb_radar_get_max_range +wb_radar_get_min_range +wb_radar_get_number_of_targets +wb_radar_get_sampling_period +wb_radar_get_targets +wb_radar_get_vertical_fov +wb_range_finder_enable +wb_range_finder_disable +wb_range_finder_get_sampling_period +wb_range_finder_get_range_image +wb_range_finder_get_width +wb_range_finder_get_height +wb_range_finder_get_fov +wb_range_finder_get_min_range +wb_range_finder_get_max_range +wb_range_finder_save_image +wb_range_finder_image_get_depth +wb_remote_control_custom_function +wb_receiver_disable +wb_receiver_enable +wb_receiver_get_channel +wb_receiver_get_data +wb_receiver_get_data_size +wb_receiver_get_emitter_direction +wb_receiver_get_queue_length +wb_receiver_get_sampling_period +wb_receiver_get_signal_strength +wb_receiver_next_packet +wb_receiver_set_channel +wb_robot_battery_sensor_disable +wb_robot_battery_sensor_enable +wb_robot_battery_sensor_get_value +wb_robot_battery_sensor_get_sampling_period +wb_robot_cleanup +wb_robot_get_basic_time_step +wb_robot_get_controller_name +wb_robot_get_custom_data +wb_robot_get_data +wb_robot_get_device +wb_robot_get_device_by_index +wb_robot_get_mode +wb_robot_get_model +wb_robot_get_name +wb_robot_get_number_of_devices +wb_robot_get_project_path +wb_robot_get_supervisor +wb_robot_get_synchronization +wb_robot_get_time +wb_robot_get_urdf +wb_robot_get_world_path +wb_robot_init +wb_robot_init_msvc +wb_robot_mutex_delete +wb_robot_mutex_lock +wb_robot_mutex_new +wb_robot_mutex_unlock +wb_robot_pin_to_static_environment +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 +wb_robot_wwi_send +wb_robot_wwi_receive +wb_robot_wwi_receive_text +wb_skin_get_bone_count +wb_skin_get_bone_name +wb_skin_get_bone_orientation +wb_skin_get_bone_position +wb_skin_set_bone_orientation +wb_skin_set_bone_position +wb_speaker_get_engine +wb_speaker_get_language +wb_speaker_is_sound_playing +wb_speaker_is_speaking +wb_speaker_play_sound +wb_speaker_set_engine +wb_speaker_set_language +wb_speaker_speak +wb_speaker_stop +wb_supervisor_node_add_force +wb_supervisor_node_add_force_with_offset +wb_supervisor_node_add_torque +wb_supervisor_animation_start_recording +wb_supervisor_animation_stop_recording +wb_supervisor_export_image +wb_supervisor_field_disable_sf_tracking +wb_supervisor_field_enable_sf_tracking +wb_supervisor_field_get_actual_field +wb_supervisor_field_get_count +wb_supervisor_field_get_name +wb_supervisor_field_get_mf_bool +wb_supervisor_field_get_mf_color +wb_supervisor_field_get_mf_float +wb_supervisor_field_get_mf_int32 +wb_supervisor_field_get_mf_node +wb_supervisor_field_get_mf_rotation +wb_supervisor_field_get_mf_string +wb_supervisor_field_get_mf_vec2f +wb_supervisor_field_get_mf_vec3f +wb_supervisor_field_get_sf_bool +wb_supervisor_field_get_sf_color +wb_supervisor_field_get_sf_float +wb_supervisor_field_get_sf_int32 +wb_supervisor_field_get_sf_node +wb_supervisor_field_get_sf_rotation +wb_supervisor_field_get_sf_string +wb_supervisor_field_get_sf_vec2f +wb_supervisor_field_get_sf_vec3f +wb_supervisor_field_get_type +wb_supervisor_field_get_type_name +wb_supervisor_field_import_mf_node_from_string +wb_supervisor_field_import_sf_node_from_string +wb_supervisor_field_insert_mf_bool +wb_supervisor_field_insert_mf_color +wb_supervisor_field_insert_mf_float +wb_supervisor_field_insert_mf_int32 +wb_supervisor_field_insert_mf_rotation +wb_supervisor_field_insert_mf_string +wb_supervisor_field_insert_mf_vec2f +wb_supervisor_field_insert_mf_vec3f +wb_supervisor_field_remove_mf +wb_supervisor_field_remove_mf_node +wb_supervisor_field_remove_sf +wb_supervisor_field_set_mf_bool +wb_supervisor_field_set_mf_color +wb_supervisor_field_set_mf_float +wb_supervisor_field_set_mf_int32 +wb_supervisor_field_set_mf_rotation +wb_supervisor_field_set_mf_string +wb_supervisor_field_set_mf_vec2f +wb_supervisor_field_set_mf_vec3f +wb_supervisor_field_set_sf_bool +wb_supervisor_field_set_sf_color +wb_supervisor_field_set_sf_float +wb_supervisor_field_set_sf_int32 +wb_supervisor_field_set_sf_rotation +wb_supervisor_field_set_sf_string +wb_supervisor_field_set_sf_vec2f +wb_supervisor_field_set_sf_vec3f +wb_supervisor_movie_failed +wb_supervisor_movie_is_ready +wb_supervisor_movie_start_recording +wb_supervisor_movie_stop_recording +wb_supervisor_movie_get_status +wb_supervisor_node_disable_contact_point_tracking +wb_supervisor_node_disable_contact_points_tracking +wb_supervisor_node_disable_pose_tracking +wb_supervisor_node_enable_contact_point_tracking +wb_supervisor_node_enable_contact_points_tracking +wb_supervisor_node_enable_pose_tracking +wb_supervisor_node_export_string +wb_supervisor_node_get_base_node_field +wb_supervisor_node_get_base_node_field_by_index +wb_supervisor_node_get_base_type_name +wb_supervisor_node_get_def +wb_supervisor_node_get_field +wb_supervisor_node_get_field_by_index +wb_supervisor_node_get_from_def +wb_supervisor_node_get_from_id +wb_supervisor_node_get_from_proto_def +wb_supervisor_node_get_from_device +wb_supervisor_node_get_center_of_mass +wb_supervisor_node_get_contact_point +wb_supervisor_node_get_contact_points +wb_supervisor_node_get_contact_point_node +wb_supervisor_node_get_id +wb_supervisor_node_get_number_of_base_node_fields +wb_supervisor_node_get_number_of_contact_points +wb_supervisor_node_get_number_of_fields +wb_supervisor_node_get_orientation +wb_supervisor_node_get_parent_node +wb_supervisor_node_get_pose +wb_supervisor_node_get_position +wb_supervisor_node_get_proto +wb_supervisor_node_get_root +wb_supervisor_node_get_selected +wb_supervisor_node_get_self +wb_supervisor_node_get_static_balance +wb_supervisor_node_get_type +wb_supervisor_node_get_type_name +wb_supervisor_node_get_velocity +wb_supervisor_node_is_proto +wb_supervisor_node_load_state +wb_supervisor_node_move_viewpoint +wb_supervisor_node_remove +wb_supervisor_node_reset_physics +wb_supervisor_node_restart_controller +wb_supervisor_node_save_state +wb_supervisor_node_set_joint_position +wb_supervisor_node_set_velocity +wb_supervisor_node_set_visibility +wb_supervisor_proto_get_field +wb_supervisor_proto_get_field_by_index +wb_supervisor_proto_get_number_of_fields +wb_supervisor_proto_get_parent +wb_supervisor_proto_get_type_name +wb_supervisor_proto_is_derived +wb_supervisor_set_label +wb_supervisor_simulation_get_mode +wb_supervisor_simulation_physics_reset +wb_supervisor_simulation_quit +wb_supervisor_simulation_reset +wb_supervisor_simulation_reset_physics +wb_supervisor_simulation_revert +wb_supervisor_simulation_set_mode +wb_supervisor_start_movie +wb_supervisor_stop_movie +wb_supervisor_get_movie_status +wb_supervisor_virtual_reality_headset_get_position +wb_supervisor_virtual_reality_headset_get_orientation +wb_supervisor_virtual_reality_headset_is_used +wb_supervisor_world_load +wb_supervisor_world_save +wb_supervisor_world_reload +wb_touch_sensor_disable +wb_touch_sensor_enable +wb_touch_sensor_get_sampling_period +wb_touch_sensor_get_type +wb_touch_sensor_get_value +wb_touch_sensor_get_values +wb_touch_sensor_get_lookup_table_size +wb_touch_sensor_get_lookup_table +wb_vacuum_gripper_disable_presence +wb_vacuum_gripper_enable_presence +wb_vacuum_gripper_get_presence +wb_vacuum_gripper_get_presence_sampling_period +wb_vacuum_gripper_is_on +wb_vacuum_gripper_turn_on +wb_vacuum_gripper_turn_off +wbr_accelerometer_set_values +wbr_camera_get_image_buffer +wbr_camera_set_image +wbr_camera_set_focal_distance +wbr_camera_set_fov +wbr_compass_set_values +wbr_display_save_image +wbr_distance_sensor_set_value +wbr_gps_set_values +wbr_gyro_set_values +wbr_light_sensor_set_value +wbr_microphone_set_buffer +wbr_motor_set_force_feedback +wbr_motor_set_torque_feedback +wbr_position_sensor_set_value +wbr_robot_battery_sensor_set_value +wbr_touch_sensor_set_value +wbr_touch_sensor_set_values +wbu_default_robot_window_configure +wbu_default_robot_window_update +wbu_default_robot_window_set_images_max_size +wbu_motion_delete +wbu_motion_get_duration +wbu_motion_get_time +wbu_motion_is_over +wbu_motion_new +wbu_motion_play +wbu_motion_set_loop +wbu_motion_set_reverse +wbu_motion_set_time +wbu_motion_stop +wbu_robot_window_configure +wbu_robot_window_update +wbu_string_replace +wbu_string_strsep +wbu_system_getenv +wbu_system_short_path diff --git a/src/controller/c/README.TXT b/src/controller/c/README.TXT new file mode 100644 index 0000000..a026e91 --- /dev/null +++ b/src/controller/c/README.TXT @@ -0,0 +1,5 @@ +api module + +This module contains all the controller API functions for controlling a robot +and handling user interaction. Moreover, it contains networking and scene +management stuff. diff --git a/src/controller/c/abstract_camera.c b/src/controller/c/abstract_camera.c new file mode 100644 index 0000000..4b882ab --- /dev/null +++ b/src/controller/c/abstract_camera.c @@ -0,0 +1,171 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "abstract_camera.h" + +#ifdef _WIN32 +#include +#else // memory mapped files +#include +#include +#include +#endif +#include +#include +#include +#include +#include "messages.h" +#include "robot_private.h" + +void wb_abstract_camera_cleanup(WbDevice *d) { + AbstractCamera *c = d->pdata; + if (c == NULL) + return; + image_cleanup(c->image); + free(c->image); + free(c); +} + +void wb_abstract_camera_new(WbDevice *d, unsigned int id, int w, int h, double fov, double camnear, bool planar) { + wb_abstract_camera_cleanup(d); + AbstractCamera *c = malloc(sizeof(AbstractCamera)); + c->enable = false; + c->unique_id = id; + c->width = w; + c->height = h; + c->fov = fov; + c->camnear = camnear; + c->planar = planar; + c->sampling_period = 0; + c->image = image_new(); + + d->pdata = c; +} + +void wb_abstract_camera_write_request(WbDevice *d, WbRequest *r) { + AbstractCamera *c = d->pdata; + if (c->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, c->sampling_period); + c->enable = false; // done + } +} + +bool wb_abstract_camera_handle_command(WbDevice *d, WbRequest *r, unsigned char command) { + bool command_handled = true; + AbstractCamera *c = d->pdata; + + switch (command) { + case C_CAMERA_MEMORY_MAPPED_FILE: + // Cleanup the previous memory mapped file if any. + image_cleanup(c->image); + image_setup(c->image, r); + break; + + default: + command_handled = false; + break; + } + return command_handled; +} + +void abstract_camera_toggle_remote(WbDevice *d, WbRequest *r) { + AbstractCamera *c = d->pdata; + if (c->sampling_period != 0) + c->enable = true; +} + +void wbr_abstract_camera_set_image(WbDevice *d, const unsigned char *image) { + AbstractCamera *c = d->pdata; + if (c && c->image->data) + memcpy(c->image->data, image, 4 * c->height * c->width); +} + +unsigned char *wbr_abstract_camera_get_image_buffer(WbDevice *d) { + AbstractCamera *c = d->pdata; + if (c && c->image->data) + return c->image->data; + return NULL; +} + +void abstract_camera_allocate_image(WbDevice *d, int size) { + AbstractCamera *c = d->pdata; + if (c) { + c->image->data = realloc(c->image->data, size); + c->image->size = size; + } +} + +void wb_abstract_camera_enable(WbDevice *d, int sampling_period) { + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + + if (ac) { + ac->enable = true; + ac->sampling_period = sampling_period; + } + robot_mutex_unlock(); +} + +int wb_abstract_camera_get_sampling_period(const WbDevice *d) { + int sampling_period = 0; + robot_mutex_lock(); + const AbstractCamera *ac = d->pdata; + if (ac) + sampling_period = ac->sampling_period; + robot_mutex_unlock(); + return sampling_period; +} + +int wb_abstract_camera_get_height(const WbDevice *d) { + int result = -1; + robot_mutex_lock(); + const AbstractCamera *ac = d->pdata; + if (ac) + result = ac->height; + robot_mutex_unlock(); + return result; +} + +int wb_abstract_camera_get_width(const WbDevice *d) { + int result = -1; + robot_mutex_lock(); + const AbstractCamera *ac = d->pdata; + if (ac) + result = ac->width; + robot_mutex_unlock(); + return result; +} + +double wb_abstract_camera_get_fov(const WbDevice *d) { + double result = NAN; + robot_mutex_lock(); + const AbstractCamera *ac = d->pdata; + if (ac) + result = ac->fov; + robot_mutex_unlock(); + return result; +} + +double wb_abstract_camera_get_near(const WbDevice *d) { + double result = NAN; + robot_mutex_lock(); + const AbstractCamera *ac = d->pdata; + if (ac) + result = ac->camnear; + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/abstract_camera.h b/src/controller/c/abstract_camera.h new file mode 100644 index 0000000..676ed90 --- /dev/null +++ b/src/controller/c/abstract_camera.h @@ -0,0 +1,64 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef ABSTRACT_CAMERA_PRIVATE_H +#define ABSTRACT_CAMERA_PRIVATE_H + +#include + +#include "device_private.h" +#include "image_private.h" + +#ifdef _WIN32 +#include +#endif + +typedef struct { + bool enable; + int sampling_period; + unsigned int unique_id; // camera id + int width; + int height; + double camnear; + bool planar; + double fov; // in degrees + int mode; + void *pdata; + Image *image; +} AbstractCamera; + +void wb_abstract_camera_cleanup(WbDevice *d); + +void wb_abstract_camera_new(WbDevice *d, unsigned int id, int w, int h, double fov, double camnear, bool planar); + +void wb_abstract_camera_write_request(WbDevice *d, WbRequest *r); +bool wb_abstract_camera_handle_command(WbDevice *d, WbRequest *r, unsigned char command); + +void abstract_camera_toggle_remote(WbDevice *d, WbRequest *r); + +void wbr_abstract_camera_set_image(WbDevice *d, const unsigned char *image); +unsigned char *wbr_abstract_camera_get_image_buffer(WbDevice *d); + +void abstract_camera_allocate_image(WbDevice *d, int size); + +void wb_abstract_camera_enable(WbDevice *d, int sampling_period); +int wb_abstract_camera_get_sampling_period(const WbDevice *d); +int wb_abstract_camera_get_height(const WbDevice *d); +int wb_abstract_camera_get_width(const WbDevice *d); +double wb_abstract_camera_get_fov(const WbDevice *d); +double wb_abstract_camera_get_near(const WbDevice *d); + +#endif // ABSTRACT_CAMERA_PRIVATE_H diff --git a/src/controller/c/accelerometer.c b/src/controller/c/accelerometer.c new file mode 100644 index 0000000..0228bf4 --- /dev/null +++ b/src/controller/c/accelerometer.c @@ -0,0 +1,191 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 // malloc and free +#include +#include +#include +#include "device_private.h" +#include "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool enable; // need to enable device ? + int sampling_period; // milliseconds + double values[3]; // acceleration + int lookup_table_size; + double *lookup_table; +} Accelerometer; + +static Accelerometer *accelerometer_create() { + Accelerometer *acc = malloc(sizeof(Accelerometer)); + acc->enable = false; + acc->sampling_period = 0; + acc->values[0] = NAN; + acc->values[1] = NAN; + acc->values[2] = NAN; + acc->lookup_table = NULL; + acc->lookup_table_size = 0; + return acc; +} + +static Accelerometer *accelerometer_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_ACCELEROMETER, true); + return d ? d->pdata : NULL; +} + +static void accelerometer_read_answer(WbDevice *d, WbRequest *r) { + Accelerometer *acc = d->pdata; + switch (request_read_uchar(r)) { + case C_ACCELEROMETER_DATA: + acc->values[0] = request_read_double(r); + acc->values[1] = request_read_double(r); + acc->values[2] = request_read_double(r); + break; + case C_CONFIGURE: + acc->lookup_table_size = request_read_int32(r); + free(acc->lookup_table); + acc->lookup_table = NULL; + if (acc->lookup_table_size > 0) { + acc->lookup_table = (double *)malloc(sizeof(double) * acc->lookup_table_size * 3); + for (int i = 0; i < acc->lookup_table_size * 3; i++) + acc->lookup_table[i] = request_read_double(r); + } + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +int wb_accelerometer_get_lookup_table_size(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const Accelerometer *dev = accelerometer_get_struct(tag); + if (dev) + result = dev->lookup_table_size; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_accelerometer_get_lookup_table(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const Accelerometer *dev = accelerometer_get_struct(tag); + if (dev) + result = dev->lookup_table; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void accelerometer_write_request(WbDevice *d, WbRequest *r) { + Accelerometer *acc = d->pdata; + if (acc->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, acc->sampling_period); + acc->enable = false; // done + } +} + +static void accelerometer_cleanup(WbDevice *d) { + Accelerometer *acc = (Accelerometer *)d->pdata; + free(acc->lookup_table); + free(d->pdata); +} + +static void accelerometer_toggle_remote(WbDevice *d, WbRequest *r) { + Accelerometer *acc = d->pdata; + if (acc->sampling_period != 0) + acc->enable = true; +} + +void wbr_accelerometer_set_values(WbDeviceTag tag, const double *values) { + Accelerometer *acc = accelerometer_get_struct(tag); + if (acc) { + acc->values[0] = values[0]; + acc->values[1] = values[1]; + acc->values[2] = values[2]; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Protected functions (exported to device.cc) + +void wb_accelerometer_init(WbDevice *d) { + d->pdata = accelerometer_create(); + d->write_request = accelerometer_write_request; + d->read_answer = accelerometer_read_answer; + d->cleanup = accelerometer_cleanup; + d->toggle_remote = accelerometer_toggle_remote; +} + +// Public function available from the user API + +void wb_accelerometer_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Accelerometer *acc = accelerometer_get_struct(tag); + if (acc) { + acc->sampling_period = sampling_period; + acc->enable = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_accelerometer_disable(WbDeviceTag tag) { + const Accelerometer *acc = accelerometer_get_struct(tag); + if (acc) + wb_accelerometer_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_accelerometer_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Accelerometer *acc = accelerometer_get_struct(tag); + if (acc) + sampling_period = acc->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +const double *wb_accelerometer_get_values(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const Accelerometer *acc = accelerometer_get_struct(tag); + if (acc) { + if (acc->sampling_period == 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_accelerometer_enable().\n", __FUNCTION__); + result = acc->values; + } + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/altimeter.c b/src/controller/c/altimeter.c new file mode 100644 index 0000000..caa64e0 --- /dev/null +++ b/src/controller/c/altimeter.c @@ -0,0 +1,144 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool enable; + int sampling_period; + double altitude; +} Altimeter; + +static Altimeter *altimeter_create() { + Altimeter *altimeter = malloc(sizeof(Altimeter)); + altimeter->enable = false; + altimeter->sampling_period = 0; + altimeter->altitude = NAN; + return altimeter; +} + +static Altimeter *altimeter_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_ALTIMETER, true); + return d ? d->pdata : NULL; +} + +static void altimeter_read_answer(WbDevice *d, WbRequest *r) { + Altimeter *altimeter = d->pdata; + + switch (request_read_uchar(r)) { + case C_ALTIMETER_DATA: + altimeter->altitude = request_read_double(r); + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +static void altimeter_write_request(WbDevice *d, WbRequest *r) { + Altimeter *altimeter = d->pdata; + if (altimeter->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, altimeter->sampling_period); + altimeter->enable = false; + } +} + +static void altimeter_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void altimeter_toggle_remote(WbDevice *d, WbRequest *r) { + Altimeter *altimeter = d->pdata; + if (altimeter->sampling_period != 0) + altimeter->enable = true; +} + +void wbr_altimeter_set_value(WbDeviceTag t, const double value) { + Altimeter *altimeter = altimeter_get_struct(t); + if (altimeter) { + altimeter->altitude = value; + } else + fprintf(stderr, "Error: %s(): invalid device tag. \n", __FUNCTION__); +} + +// Protected functions (exported to device.cc) + +void wb_altimeter_init(WbDevice *d) { + d->write_request = altimeter_write_request; + d->read_answer = altimeter_read_answer; + d->cleanup = altimeter_cleanup; + d->pdata = altimeter_create(); + d->toggle_remote = altimeter_toggle_remote; +} + +// Public function available from the user API + +void wb_altimeter_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period. \n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Altimeter *altimeter = altimeter_get_struct(tag); + if (altimeter) { + altimeter->enable = true; + altimeter->sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_altimeter_disable(WbDeviceTag tag) { + const Altimeter *altimeter = altimeter_get_struct(tag); + if (altimeter) + wb_altimeter_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_altimeter_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Altimeter *altimeter = altimeter_get_struct(tag); + if (altimeter) + sampling_period = altimeter->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +double wb_altimeter_get_value(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Altimeter *altimeter = altimeter_get_struct(tag); + if (altimeter) { + if (altimeter->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_altimeter_enable().\n", __FUNCTION__); + result = altimeter->altitude; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/ansi_codes.c b/src/controller/c/ansi_codes.c new file mode 100644 index 0000000..5e3cee1 --- /dev/null +++ b/src/controller/c/ansi_codes.c @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 + +const char *wb_ANSI_RESET = ANSI_RESET, *wb_ANSI_BOLD = ANSI_BOLD, *wb_ANSI_UNDERLINE = ANSI_UNDERLINE, + *wb_ANSI_BLACK_BACKGROUND = ANSI_BLACK_BACKGROUND, *wb_ANSI_RED_BACKGROUND = ANSI_RED_BACKGROUND, + *wb_ANSI_GREEN_BACKGROUND = ANSI_GREEN_BACKGROUND, *wb_ANSI_YELLOW_BACKGROUND = ANSI_YELLOW_BACKGROUND, + *wb_ANSI_BLUE_BACKGROUND = ANSI_BLUE_BACKGROUND, *wb_ANSI_MAGENTA_BACKGROUND = ANSI_MAGENTA_BACKGROUND, + *wb_ANSI_CYAN_BACKGROUND = ANSI_CYAN_BACKGROUND, *wb_ANSI_WHITE_BACKGROUND = ANSI_WHITE_BACKGROUND, + *wb_ANSI_BLACK_FOREGROUND = ANSI_BLACK_FOREGROUND, *wb_ANSI_RED_FOREGROUND = ANSI_RED_FOREGROUND, + *wb_ANSI_GREEN_FOREGROUND = ANSI_GREEN_FOREGROUND, *wb_ANSI_YELLOW_FOREGROUND = ANSI_YELLOW_FOREGROUND, + *wb_ANSI_BLUE_FOREGROUND = ANSI_BLUE_FOREGROUND, *wb_ANSI_MAGENTA_FOREGROUND = ANSI_MAGENTA_FOREGROUND, + *wb_ANSI_CYAN_FOREGROUND = ANSI_CYAN_FOREGROUND, *wb_ANSI_WHITE_FOREGROUND = ANSI_WHITE_FOREGROUND, + *wb_ANSI_CLEAR_SCREEN = ANSI_CLEAR_SCREEN; diff --git a/src/controller/c/base64.c b/src/controller/c/base64.c new file mode 100644 index 0000000..1496dfb --- /dev/null +++ b/src/controller/c/base64.c @@ -0,0 +1,59 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +// src: https://stackoverflow.com/questions/26175293/how-to-convert-image-to-base64-in-c + +#include +#include +#include +#include +#include + +static int mod_table[] = {0, 2, 1}; +static char encoding_table[] = {'A', 'B', 'C', 'D', 'E', 'F', 'G', 'H', 'I', 'J', 'K', 'L', 'M', 'N', 'O', 'P', + 'Q', 'R', 'S', 'T', 'U', 'V', 'W', 'X', 'Y', 'Z', 'a', 'b', 'c', 'd', 'e', 'f', + 'g', 'h', 'i', 'j', 'k', 'l', 'm', 'n', 'o', 'p', 'q', 'r', 's', 't', 'u', 'v', + 'w', 'x', 'y', 'z', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', '+', '/'}; + +char *base64_encode(const unsigned char *data, size_t input_length, size_t *output_length) { + *output_length = 4 * ((input_length + 2) / 3); + + char *encoded_data = malloc(*output_length + 1); + if (encoded_data == NULL) + return NULL; + + int i; + int j = 0; + for (i = 0; i < input_length;) { + uint32_t octet_a = i < input_length ? (unsigned char)data[i++] : 0; + uint32_t octet_b = i < input_length ? (unsigned char)data[i++] : 0; + uint32_t octet_c = i < input_length ? (unsigned char)data[i++] : 0; + + uint32_t triple = (octet_a << 0x10) + (octet_b << 0x08) + octet_c; + + encoded_data[j++] = encoding_table[(triple >> 3 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 2 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 1 * 6) & 0x3F]; + encoded_data[j++] = encoding_table[(triple >> 0 * 6) & 0x3F]; + } + + for (i = 0; i < mod_table[input_length % 3]; ++i) + encoded_data[*output_length - 1 - i] = '='; + + encoded_data[*output_length] = 0; + + return encoded_data; +} diff --git a/src/controller/c/base64.h b/src/controller/c/base64.h new file mode 100644 index 0000000..1da4e57 --- /dev/null +++ b/src/controller/c/base64.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef BASE64_H +#define BASE64_H + +char *base64_encode(const unsigned char *data, size_t input_length, size_t *output_length); + +#endif diff --git a/src/controller/c/brake.c b/src/controller/c/brake.c new file mode 100644 index 0000000..412bd80 --- /dev/null +++ b/src/controller/c/brake.c @@ -0,0 +1,164 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +//*************************************************************************** +// this file contains the API interface for the Brake device +//*************************************************************************** + +#include +#include +#include +#include +#include +#include "messages.h" +#include "robot_private.h" + +typedef struct { + int state; + WbJointType type; + double damping_constant; + int requested_device_type; + int requested_device_tag; +} Brake; + +static Brake *brake_create() { + Brake *brake = malloc(sizeof(Brake)); + brake->state = 0; + brake->type = WB_ROTATIONAL; + brake->damping_constant = 0.0; + brake->requested_device_type = 0; + brake->requested_device_tag = 0; + return brake; +} + +// Static functions +static Brake *brake_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_BRAKE, true); + return d ? d->pdata : NULL; +} + +static void brake_write_request(WbDevice *d, WbRequest *r) { + Brake *b = (Brake *)d->pdata; + if (b->state) { + request_write_uchar(r, b->state); + if (b->state & C_BRAKE_SET_DAMPING_CONSTANT) + request_write_double(r, b->damping_constant); + if (b->state & C_BRAKE_GET_ASSOCIATED_DEVICE) { + request_write_uint16(r, b->requested_device_type); + b->requested_device_type = 0; + } + b->state = 0; // clear it for next time + } +} + +static void brake_read_answer(WbDevice *d, WbRequest *r) { + Brake *b = (Brake *)d->pdata; + switch (request_read_uchar(r)) { + case C_CONFIGURE: + b->type = request_read_int32(r); + break; + case C_BRAKE_GET_ASSOCIATED_DEVICE: + b->requested_device_tag = request_read_uint16(r); + break; + default: + ROBOT_ASSERT(0); // should not be reached + break; + } +} + +static void brake_cleanup(WbDevice *d) { + Brake *b = (Brake *)d->pdata; + free(b); +} + +static void brake_toggle_remote(WbDevice *d, WbRequest *r) { + // nop: reseting the position_sensor is often problematic +} + +// Exported functions + +void wb_brake_init(WbDevice *d) { + d->read_answer = brake_read_answer; + d->write_request = brake_write_request; + d->cleanup = brake_cleanup; + d->toggle_remote = brake_toggle_remote; + d->pdata = brake_create(); +} + +// Public functions (available from the user API) + +void wb_brake_set_damping_constant_no_mutex(WbDeviceTag tag, double damping_constant) { + Brake *b = brake_get_struct(tag); + if (b) { + b->state |= C_BRAKE_SET_DAMPING_CONSTANT; + b->damping_constant = damping_constant; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wb_brake_set_damping_constant(WbDeviceTag tag, double damping_constant) { + if (isnan(damping_constant)) { + fprintf(stderr, "Error: %s() called with an invalid 'damping_constant' argument (NaN).\n", __FUNCTION__); + return; + } + + const Brake *b = brake_get_struct(tag); + if (!b) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + wb_brake_set_damping_constant_no_mutex(tag, damping_constant); + robot_mutex_unlock(); +} + +WbJointType wb_brake_get_type(WbDeviceTag tag) { + WbJointType type = WB_ROTATIONAL; + robot_mutex_lock(); + const Brake *b = brake_get_struct(tag); + if (b) + type = b->type; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return type; +} + +static WbDeviceTag brake_get_associated_device(WbDeviceTag t, int device_type, const char *function_name) { + Brake *b = brake_get_struct(t); + if (!b) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", function_name); + return 0; + } + + robot_mutex_lock(); + b->state |= C_BRAKE_GET_ASSOCIATED_DEVICE; + b->requested_device_type = device_type; + wb_robot_flush_unlocked(function_name); + WbDeviceTag result = b->requested_device_tag; + robot_mutex_unlock(); + return result; +} + +WbDeviceTag wb_brake_get_motor(WbDeviceTag tag) { + // this function works for both linear and rotational motors + return brake_get_associated_device(tag, WB_NODE_ROTATIONAL_MOTOR, __FUNCTION__); +} + +WbDeviceTag wb_brake_get_position_sensor(WbDeviceTag tag) { + return brake_get_associated_device(tag, WB_NODE_POSITION_SENSOR, __FUNCTION__); +} diff --git a/src/controller/c/camera.c b/src/controller/c/camera.c new file mode 100644 index 0000000..ff337aa --- /dev/null +++ b/src/controller/c/camera.c @@ -0,0 +1,894 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "abstract_camera.h" +#include "camera_private.h" +#include "g_image.h" +#include "messages.h" +#include "remote_control_private.h" +#include "robot_private.h" + +static WbDevice *camera_get_device(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_CAMERA, true); + return d; +} + +static AbstractCamera *camera_get_abstract_camera_struct(WbDeviceTag t) { + WbDevice *d = camera_get_device(t); + return d ? d->pdata : NULL; +} + +static Camera *camera_get_struct(WbDeviceTag tag) { + AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + return ac ? ac->pdata : NULL; +} + +static void camera_clear_recognized_objects_list(Camera *c) { + int i; + for (i = 0; i < c->recognized_object_number; ++i) { + free(c->recognized_objects[i].colors); + free(c->recognized_objects[i].model); + } + free(c->recognized_objects); +} + +static void wb_camera_cleanup(WbDevice *d) { + AbstractCamera *ac = d->pdata; + if (!ac) + return; + Camera *c = ac->pdata; + if (!c) + return; + camera_clear_recognized_objects_list(c); + if (c->segmentation_image) { + image_cleanup(c->segmentation_image); + free(c->segmentation_image); + } + free(c); + ac->pdata = NULL; + wb_abstract_camera_cleanup(d); +} + +static void wb_camera_new(WbDevice *d, unsigned int id, int w, int h, double fov, double min_fov, double max_fov, + double exposure, double focal_length, double focal_distance, double min_focal_distance, + double max_focal_distance, double camnear, bool planar, bool has_recognition, bool segmentation) { + Camera *c; + wb_camera_cleanup(d); + wb_abstract_camera_new(d, id, w, h, fov, camnear, planar); + + c = malloc(sizeof(Camera)); + c->min_fov = min_fov; + c->max_fov = max_fov; + c->exposure = exposure; + c->focal_length = focal_length; + c->focal_distance = focal_distance; + c->min_focal_distance = min_focal_distance; + c->max_focal_distance = max_focal_distance; + c->has_recognition = has_recognition; + c->set_focal_distance = false; + c->set_fov = false; + c->set_exposure = false; + c->enable_recognition = false; + c->recognition_sampling_period = 0; + c->recognized_object_number = 0; + c->recognized_objects = NULL; + c->segmentation = segmentation; + c->segmentation_enabled = false; + c->segmentation_changed = false; + if (c->segmentation) + c->segmentation_image = image_new(); + else + c->segmentation_image = NULL; + + AbstractCamera *ac = d->pdata; + ac->pdata = c; +} + +static void wb_camera_write_request(WbDevice *d, WbRequest *r) { + wb_abstract_camera_write_request(d, r); + AbstractCamera *ac = d->pdata; + Camera *c = ac->pdata; + if (c->set_fov) { + request_write_uchar(r, C_CAMERA_SET_FOV); + request_write_double(r, ac->fov); + c->set_fov = false; // done + } + if (c->set_exposure) { + request_write_uchar(r, C_CAMERA_SET_EXPOSURE); + request_write_double(r, c->exposure); + c->set_exposure = false; // done + } + if (c->set_focal_distance) { + request_write_uchar(r, C_CAMERA_SET_FOCAL); + request_write_double(r, c->focal_distance); + c->set_focal_distance = false; // done + } + if (c->enable_recognition) { + request_write_uchar(r, C_CAMERA_SET_RECOGNITION_SAMPLING_PERIOD); + request_write_uint16(r, c->recognition_sampling_period); + c->enable_recognition = false; // done + } + if (c->segmentation_changed) { + request_write_uchar(r, C_CAMERA_ENABLE_SEGMENTATION); + request_write_uchar(r, c->segmentation_enabled ? 1 : 0); + if (c->segmentation_enabled && !c->segmentation_image) + c->segmentation_image = image_new(); + c->segmentation_changed = false; // done + } +} + +static void wb_camera_read_answer(WbDevice *d, WbRequest *r) { + unsigned char command = request_read_uchar(r); + if (wb_abstract_camera_handle_command(d, r, command)) + return; + unsigned int uid; + int width, height; + double fov, min_fov, max_fov, camnear, exposure, focal_length, focal_distance, min_focal_distance, max_focal_distance; + bool planar, has_recognition, segmentation; + + AbstractCamera *ac = d->pdata; + Camera *c = NULL; + + switch (command) { + case C_CONFIGURE: + uid = request_read_uint32(r); + width = request_read_uint16(r); + height = request_read_uint16(r); + fov = request_read_double(r); + camnear = request_read_double(r); + planar = request_read_uchar(r); + min_fov = request_read_double(r); + max_fov = request_read_double(r); + has_recognition = request_read_uchar(r) != 0; + segmentation = request_read_uchar(r) != 0; + exposure = request_read_double(r); + focal_length = request_read_double(r); + focal_distance = request_read_double(r); + min_focal_distance = request_read_double(r); + max_focal_distance = request_read_double(r); + + // printf("new camera %u %d %d %lf %lf %d\n", uid, width, height, fov, camnear, planar); + wb_camera_new(d, uid, width, height, fov, min_fov, max_fov, exposure, focal_length, focal_distance, min_focal_distance, + max_focal_distance, camnear, planar, has_recognition, segmentation); + break; + case C_CAMERA_RECONFIGURE: + c = ac->pdata; + ac->fov = request_read_double(r); + ac->camnear = request_read_double(r); + ac->planar = request_read_uchar(r); + c->min_fov = request_read_double(r); + c->max_fov = request_read_double(r); + c->has_recognition = request_read_uchar(r) != 0; + c->segmentation = request_read_uchar(r) != 0; + c->exposure = request_read_double(r); + c->focal_length = request_read_double(r); + c->focal_distance = request_read_double(r); + c->min_focal_distance = request_read_double(r); + c->max_focal_distance = request_read_double(r); + break; + case C_CAMERA_OBJECTS: { + c = ac->pdata; + int i, j; + + // clean previous list + camera_clear_recognized_objects_list(c); + // get number of recognized objects + c->recognized_object_number = request_read_int32(r); + c->recognized_objects = + (WbCameraRecognitionObject *)malloc(c->recognized_object_number * sizeof(WbCameraRecognitionObject)); + + for (i = 0; i < c->recognized_object_number; ++i) { + // get id of the object + c->recognized_objects[i].id = request_read_int32(r); + // get relative position of the object + c->recognized_objects[i].position[0] = request_read_double(r); + c->recognized_objects[i].position[1] = request_read_double(r); + c->recognized_objects[i].position[2] = request_read_double(r); + // get relative orientation of the object + c->recognized_objects[i].orientation[0] = request_read_double(r); + c->recognized_objects[i].orientation[1] = request_read_double(r); + c->recognized_objects[i].orientation[2] = request_read_double(r); + c->recognized_objects[i].orientation[3] = request_read_double(r); + // get size of the object + c->recognized_objects[i].size[0] = request_read_double(r); + c->recognized_objects[i].size[1] = request_read_double(r); + // get position of the object on the camera image + c->recognized_objects[i].position_on_image[0] = request_read_int32(r); + c->recognized_objects[i].position_on_image[1] = request_read_int32(r); + // get size of the object on the camera image + c->recognized_objects[i].size_on_image[0] = request_read_int32(r); + c->recognized_objects[i].size_on_image[1] = request_read_int32(r); + // get number of colors of the object + c->recognized_objects[i].number_of_colors = request_read_int32(r); + const int size = 3 * c->recognized_objects[i].number_of_colors * sizeof(double *); + c->recognized_objects[i].colors = (double *)malloc(size); + for (j = 0; j < c->recognized_objects[i].number_of_colors; j++) { + // get each color of the object + c->recognized_objects[i].colors[3 * j] = request_read_double(r); + c->recognized_objects[i].colors[3 * j + 1] = request_read_double(r); + c->recognized_objects[i].colors[3 * j + 2] = request_read_double(r); + } + // get the model of the object + c->recognized_objects[i].model = request_read_string(r); + } + break; + } + case C_CAMERA_SET_SEGMENTATION: + c = ac->pdata; + c->segmentation = request_read_uchar(r); + if (!c->segmentation) + c->segmentation_enabled = false; + break; + case C_CAMERA_SEGMENTATION_MEMORY_MAPPED_FILE: + // Cleanup the previous memory mapped file if any. + c = ac->pdata; + assert(c->segmentation); + if (!c->segmentation_image) + c->segmentation_image = image_new(); // prevent controller crash + image_cleanup(c->segmentation_image); + image_setup(c->segmentation_image, r); + break; + default: + ROBOT_ASSERT(0); + break; + } +} + +static void camera_toggle_remote(WbDevice *d, WbRequest *r) { + abstract_camera_toggle_remote(d, r); + AbstractCamera *ac = d->pdata; + Camera *c = ac->pdata; + if (ac->sampling_period != 0) { + ac->enable = true; + if (remote_control_is_function_defined("wbr_camera_set_fov")) + c->set_fov = true; + if (remote_control_is_function_defined("wbr_camera_set_exposure")) + c->set_exposure = true; + if (remote_control_is_function_defined("wbr_camera_set_focal_distance")) + c->set_focal_distance = true; + } + if (c->recognition_sampling_period != 0) + c->enable_recognition = true; +} + +// Protected functions available from other source files + +void wb_camera_init(WbDevice *d) { + d->read_answer = wb_camera_read_answer; + d->write_request = wb_camera_write_request; + d->cleanup = wb_camera_cleanup; + d->pdata = NULL; + d->toggle_remote = camera_toggle_remote; + // g_print("camera init done\n"); +} + +void wbr_camera_set_image(WbDeviceTag tag, const unsigned char *image) { + WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + wbr_abstract_camera_set_image(d, image); +} + +unsigned char *wbr_camera_get_image_buffer(WbDeviceTag tag) { + WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wbr_abstract_camera_get_image_buffer(d); +} + +void wbr_camera_recognition_set_object(WbDeviceTag tag, const WbCameraRecognitionObject *objects, int object_number) { + Camera *c = camera_get_struct(tag); + if (c) { + camera_clear_recognized_objects_list(c); + // get number of recognized objects + c->recognized_object_number = object_number; + c->recognized_objects = + (WbCameraRecognitionObject *)malloc(c->recognized_object_number * sizeof(WbCameraRecognitionObject)); + int i, j; + for (i = 0; i < c->recognized_object_number; ++i) { + // set id of the object + c->recognized_objects[i].id = objects[i].id; + // set relative position of the object + c->recognized_objects[i].position[0] = objects[i].position[0]; + c->recognized_objects[i].position[1] = objects[i].position[1]; + c->recognized_objects[i].position[2] = objects[i].position[2]; + // set relative orientation of the object + c->recognized_objects[i].orientation[0] = objects[i].orientation[0]; + c->recognized_objects[i].orientation[1] = objects[i].orientation[1]; + c->recognized_objects[i].orientation[2] = objects[i].orientation[2]; + c->recognized_objects[i].orientation[3] = objects[i].orientation[3]; + // set size of the object + c->recognized_objects[i].size[0] = objects[i].size[0]; + c->recognized_objects[i].size[1] = objects[i].size[1]; + // set position of the object on the camera image + c->recognized_objects[i].position_on_image[0] = objects[i].position_on_image[0]; + c->recognized_objects[i].position_on_image[1] = objects[i].position_on_image[1]; + // set size of the object on the camera image + c->recognized_objects[i].size_on_image[0] = objects[i].size_on_image[0]; + c->recognized_objects[i].size_on_image[1] = objects[i].size_on_image[1]; + // set number of colors of the object + c->recognized_objects[i].number_of_colors = objects[i].number_of_colors; + const int size = 3 * c->recognized_objects[i].number_of_colors * sizeof(double *); + c->recognized_objects[i].colors = (double *)malloc(size); + for (j = 0; j < c->recognized_objects[i].number_of_colors; j++) { + // set each color of the object + c->recognized_objects[i].colors[3 * j] = objects[i].colors[0]; + c->recognized_objects[i].colors[3 * j + 1] = objects[i].colors[1]; + c->recognized_objects[i].colors[3 * j + 2] = objects[i].colors[2]; + } + // set the model of the object + c->recognized_objects[i].model = (char *)malloc(sizeof(objects[i].model)); + strcpy(c->recognized_objects[i].model, objects[i].model); + } + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Public functions available from the camera API + +void wb_camera_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + + wb_abstract_camera_enable(d, sampling_period); +} + +void wb_camera_disable(WbDeviceTag tag) { + const Camera *c = camera_get_struct(tag); + if (c) + wb_camera_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_camera_get_sampling_period(WbDeviceTag tag) { + const WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s: invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_sampling_period(d); +} + +int wb_camera_get_height(WbDeviceTag tag) { + const WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_height(d); +} + +int wb_camera_get_width(WbDeviceTag tag) { + const WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_width(d); +} + +double wb_camera_get_fov(WbDeviceTag tag) { + const WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_fov(d); +} + +double wb_camera_get_min_fov(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->min_fov; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_camera_get_max_fov(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->max_fov; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_camera_set_fov(WbDeviceTag tag, double fov) { + bool in_range = true; + robot_mutex_lock(); + AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + Camera *c = camera_get_struct(tag); + if (!ac || !c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!ac->planar && (fov < 0.0 || fov > 2.0 * M_PI)) { + fprintf(stderr, "Error: %s() called with 'fov' argument outside of the [0, 2.0*pi] range.\n", __FUNCTION__); + in_range = false; + } else if (ac->planar && (fov < 0.0 || fov > M_PI)) { + fprintf(stderr, "Error: %s() called with 'fov' argument outside of the [0, pi] range.\n", __FUNCTION__); + in_range = false; + } else if (fov < c->min_fov || fov > c->max_fov) { + fprintf(stderr, "Error: %s() out of zoom range [%f, %f].\n", __FUNCTION__, c->min_fov, c->max_fov); + in_range = false; + } + if (in_range) { + ac->fov = fov; + c->set_fov = true; + } + robot_mutex_unlock(); +} + +double wb_camera_get_focal_length(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->focal_length; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_camera_get_exposure(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->exposure; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_camera_set_exposure(WbDeviceTag tag, double exposure) { + robot_mutex_lock(); + Camera *c = camera_get_struct(tag); + if (!c) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + else if (exposure < 0) + fprintf(stderr, "Error: 'exposure' argument of %s() can't be negative.\n", __FUNCTION__); + else { + c->exposure = exposure; + c->set_exposure = true; + } + robot_mutex_unlock(); +} + +double wb_camera_get_focal_distance(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->focal_distance; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_camera_get_min_focal_distance(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->min_focal_distance; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_camera_get_max_focal_distance(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + result = c->max_focal_distance; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_camera_set_focal_distance(WbDeviceTag tag, double focal_distance) { + bool in_range = true; + robot_mutex_lock(); + const AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + Camera *c = camera_get_struct(tag); + if (!c || !ac) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } else if (!ac->planar) { + fprintf(stderr, "Error: %s() can only be called on a planar camera.\n", __FUNCTION__); + in_range = false; + } else if (focal_distance < c->min_focal_distance || focal_distance > c->max_focal_distance) { + fprintf(stderr, "Error: %s() out of focus range [%f, %f].\n", __FUNCTION__, c->min_focal_distance, c->max_focal_distance); + in_range = false; + } + if (in_range) { + c->focal_distance = focal_distance; + c->set_focal_distance = true; + } + robot_mutex_unlock(); +} + +double wb_camera_get_near(WbDeviceTag tag) { + const WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + + return wb_abstract_camera_get_near(d); +} + +void wb_camera_recognition_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Camera *c = camera_get_struct(tag); + + if (c) { + if (!c->has_recognition) + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + else { + c->enable_recognition = true; + c->recognition_sampling_period = sampling_period; + } + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + + robot_mutex_unlock(); +} + +void wb_camera_recognition_disable(WbDeviceTag tag) { + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + bool should_return = false; + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + should_return = true; + } else if (!c->has_recognition) { + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + should_return = true; + } + robot_mutex_unlock(); + if (!should_return) + wb_camera_recognition_enable(tag, 0); +} + +int wb_camera_recognition_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) { + if (!c->has_recognition) + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + else + sampling_period = c->recognition_sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +int wb_camera_recognition_get_number_of_objects(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) { + if (!c->has_recognition) + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + else if (c->recognition_sampling_period == 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_camera_recognition_enable().\n", __FUNCTION__); + else + result = c->recognized_object_number; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +bool wb_camera_has_recognition(WbDeviceTag tag) { + bool has_recognition = false; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + has_recognition = c->has_recognition; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return has_recognition; +} + +const WbCameraRecognitionObject *wb_camera_recognition_get_objects(WbDeviceTag tag) { + const WbCameraRecognitionObject *result = 0; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) { + if (!c->has_recognition) + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + else if (c->recognition_sampling_period == 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_camera_recognition_enable().\n", __FUNCTION__); + else + result = c->recognized_objects; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const unsigned char *wb_camera_get_image(WbDeviceTag tag) { + robot_mutex_lock(); + AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + + if (!ac) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + + if (ac->sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_camera_enable().\n", __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + + if (wb_robot_get_mode() == WB_MODE_REMOTE_CONTROL) { + robot_mutex_unlock(); + return ac->image->data; + } + + robot_mutex_unlock(); + return ac->image->data; +} + +int wb_camera_save_image(WbDeviceTag tag, const char *filename, int quality) { + if (!filename || !filename[0]) { + fprintf(stderr, "Error: %s() called with NULL or empty 'filename' argument.\n", __FUNCTION__); + return -1; + } + unsigned char type = g_image_get_type(filename); + if (type != G_IMAGE_PNG && type != G_IMAGE_JPEG) { + fprintf(stderr, "Error: %s() called with unsupported image format (should be PNG or JPEG).\n", __FUNCTION__); + return -1; + } + if (type == G_IMAGE_JPEG && (quality < 1 || quality > 100)) { + fprintf(stderr, "Error: %s() called with invalid 'quality' argument.\n", __FUNCTION__); + return -1; + } + + robot_mutex_lock(); + AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + + if (!ac) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return -1; + } + + if (!ac->image->data) { + robot_mutex_unlock(); + return -1; + } + GImage img; + img.width = ac->width; + img.height = ac->height; + + img.data_format = G_IMAGE_DATA_FORMAT_BGRA; + img.data = ac->image->data; + int ret = g_image_save(&img, filename, quality); + + robot_mutex_unlock(); + return ret; +} + +const WbCameraRecognitionObject *wb_camera_recognition_get_object(WbDeviceTag tag, int index) { + const Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; + } + return (wb_camera_recognition_get_objects(tag) + index); +} + +bool wb_camera_recognition_has_segmentation(WbDeviceTag tag) { + bool has_segmentation; + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (c) + has_segmentation = c->segmentation; + else { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + has_segmentation = false; + } + robot_mutex_unlock(); + return has_segmentation; +} + +void wb_camera_recognition_enable_segmentation(WbDeviceTag tag) { + robot_mutex_lock(); + Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!c->has_recognition) { + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (c->recognition_sampling_period == 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_camera_recognition_enable().\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!c->segmentation) { + fprintf(stderr, "Error: %s(): segmentation is disabled in Recognition node.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!c->segmentation_enabled) { + c->segmentation_enabled = true; + c->segmentation_changed = true; + } + robot_mutex_unlock(); +} + +void wb_camera_recognition_disable_segmentation(WbDeviceTag tag) { + robot_mutex_lock(); + Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!c->has_recognition) { + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (c->segmentation_enabled) { + c->segmentation_enabled = false; + c->segmentation_changed = true; + } + robot_mutex_unlock(); +} + +bool wb_camera_recognition_is_segmentation_enabled(WbDeviceTag tag) { + robot_mutex_lock(); + const Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return false; + } + if (!c->has_recognition) { + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + robot_mutex_unlock(); + return false; + } + const bool is_segmentation_enabled = c->segmentation_enabled; + robot_mutex_unlock(); + return is_segmentation_enabled; +} + +const unsigned char *wb_camera_recognition_get_segmentation_image(WbDeviceTag tag) { + robot_mutex_lock(); + Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + if (!c->has_recognition) { + fprintf(stderr, "Error: %s() called on a Camera without Recognition node.\n", __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + if (!c->segmentation) { + robot_mutex_unlock(); + return NULL; + } + if (!c->segmentation_enabled) { + fprintf(stderr, "Error: %s(): segmentation is disabled! Please use: wb_camera_recognition_enable_segmentation().\n", + __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + if (!c->segmentation_image->data) { + robot_mutex_unlock(); + return NULL; + } + robot_mutex_unlock(); + return c->segmentation_image->data; +} + +int wb_camera_recognition_save_segmentation_image(WbDeviceTag tag, const char *filename, int quality) { + if (!filename || !filename[0]) { + fprintf(stderr, "Error: %s() called with NULL or empty 'filename' argument.\n", __FUNCTION__); + return -1; + } + unsigned char type = g_image_get_type(filename); + if (type != G_IMAGE_PNG && type != G_IMAGE_JPEG) { + fprintf(stderr, "Error: %s() called with unsupported image format (should be PNG or JPEG).\n", __FUNCTION__); + return -1; + } + if (type == G_IMAGE_JPEG && (quality < 1 || quality > 100)) { + fprintf(stderr, "Error: %s() called with invalid 'quality' argument.\n", __FUNCTION__); + return -1; + } + + robot_mutex_lock(); + const AbstractCamera *ac = camera_get_abstract_camera_struct(tag); + Camera *c = camera_get_struct(tag); + if (!c) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return -1; + } + if (!c->segmentation_image) { + fprintf(stderr, "Error: %s() called before rendering a valid segmentation image.\n", __FUNCTION__); + robot_mutex_unlock(); + return -1; + } + + GImage img; + img.width = ac->width; + img.height = ac->height; + + img.data_format = G_IMAGE_DATA_FORMAT_BGRA; + img.data = c->segmentation_image->data; + int ret = g_image_save(&img, filename, quality); + + robot_mutex_unlock(); + return ret; +} + +void camera_allocate_segmentation_image(WbDeviceTag tag, int size) { + Camera *c = camera_get_struct(tag); + if (c) { + c->segmentation_image->data = realloc(c->segmentation_image->data, size); + c->segmentation_image->size = size; + } +} + +const unsigned char *camera_get_segmentation_image_buffer(WbDeviceTag tag) { + Camera *c = camera_get_struct(tag); + if (c) + return c->segmentation_image->data; + return NULL; +} diff --git a/src/controller/c/camera_private.h b/src/controller/c/camera_private.h new file mode 100644 index 0000000..50fcffb --- /dev/null +++ b/src/controller/c/camera_private.h @@ -0,0 +1,48 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef CAMERA_PRIVATE_H +#define CAMERA_PRIVATE_H + +#include +#include + +typedef struct { + double min_fov; + double max_fov; + double exposure; + double focal_length; + double focal_distance; + double min_focal_distance; + double max_focal_distance; + bool set_exposure; + bool set_focal_distance; + bool set_fov; + bool has_recognition; + bool enable_recognition; + int recognition_sampling_period; + int recognized_object_number; // number of object currrently recognized + WbCameraRecognitionObject *recognized_objects; // list of objects + bool segmentation; + bool segmentation_enabled; + bool segmentation_changed; + Image *segmentation_image; +} Camera; + +void camera_allocate_segmentation_image(WbDeviceTag tag, int size); +const unsigned char *camera_get_segmentation_image_buffer(WbDeviceTag tag); + +#endif // CAMERA_PRIVATE_H diff --git a/src/controller/c/compass.c b/src/controller/c/compass.c new file mode 100644 index 0000000..afbc924 --- /dev/null +++ b/src/controller/c/compass.c @@ -0,0 +1,190 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +typedef struct { + bool enable; // need to enable device ? + int sampling_period; // milliseconds + double north[3]; // north + int lookup_table_size; + double *lookup_table; +} Compass; + +static Compass *compass_create() { + Compass *compass = malloc(sizeof(Compass)); + compass->enable = false; + compass->sampling_period = 0; + compass->north[0] = NAN; + compass->north[1] = NAN; + compass->north[2] = NAN; + compass->lookup_table = NULL; + compass->lookup_table_size = 0; + return compass; +} + +// Static functions + +static Compass *compass_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_COMPASS, true); + return d ? d->pdata : NULL; +} + +static void compass_read_answer(WbDevice *d, WbRequest *r) { + Compass *compass = d->pdata; + + switch (request_read_uchar(r)) { + case C_COMPASS_DATA: + compass->north[0] = request_read_double(r); + compass->north[1] = request_read_double(r); + compass->north[2] = request_read_double(r); + break; + case C_CONFIGURE: + compass->lookup_table_size = request_read_int32(r); + free(compass->lookup_table); + compass->lookup_table = NULL; + if (compass->lookup_table_size > 0) { + compass->lookup_table = (double *)malloc(sizeof(double) * compass->lookup_table_size * 3); + for (int i = 0; i < compass->lookup_table_size * 3; i++) + compass->lookup_table[i] = request_read_double(r); + } + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +int wb_compass_get_lookup_table_size(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const Compass *dev = compass_get_struct(tag); + if (dev) + result = dev->lookup_table_size; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_compass_get_lookup_table(WbDeviceTag tag) { + double *result = NULL; + robot_mutex_lock(); + Compass *dev = compass_get_struct(tag); + if (dev) + result = dev->lookup_table; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void compass_write_request(WbDevice *d, WbRequest *r) { + Compass *compass = d->pdata; + if (compass->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, compass->sampling_period); + compass->enable = false; // done + } +} + +static void compass_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void compass_toggle_remote(WbDevice *d, WbRequest *r) { + Compass *compass = d->pdata; + if (compass->sampling_period != 0) + compass->enable = true; +} + +void wbr_compass_set_values(WbDeviceTag tag, const double *values) { + Compass *compass = compass_get_struct(tag); + if (compass) { + compass->north[0] = values[0]; + compass->north[1] = values[1]; + compass->north[2] = values[2]; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wb_compass_init(WbDevice *); + +void wb_compass_init(WbDevice *d) { + d->write_request = compass_write_request; + d->read_answer = compass_read_answer; + d->cleanup = compass_cleanup; + d->pdata = compass_create(); + d->toggle_remote = compass_toggle_remote; +} + +// Public function available from the user API + +void wb_compass_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Compass *compass = compass_get_struct(tag); + if (compass) { + compass->sampling_period = sampling_period; + compass->enable = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_compass_disable(WbDeviceTag tag) { + const Compass *compass = compass_get_struct(tag); + if (compass) + wb_compass_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_compass_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Compass *compass = compass_get_struct(tag); + if (compass) + sampling_period = compass->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +const double *wb_compass_get_values(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const Compass *compass = compass_get_struct(tag); + if (compass) { + if (compass->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_compass_enable().\n", __FUNCTION__); + result = compass->north; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/connector.c b/src/controller/c/connector.c new file mode 100644 index 0000000..17a1b98 --- /dev/null +++ b/src/controller/c/connector.c @@ -0,0 +1,187 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +//*************************************************************************** +// this file is the API code for the Connector device +//*************************************************************************** + +#include +#include +#include +#include +#include "device_private.h" +#include "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool set_locked_state; + bool is_locked; + bool enable_presence; + int presence_sampling_period; + int presence; +} Connector; + +static Connector *connector_create() { + Connector *con = malloc(sizeof(Connector)); + con->set_locked_state = false; + con->is_locked = false; + con->enable_presence = false; + con->presence_sampling_period = 0; + return con; +} + +static Connector *connector_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_CONNECTOR, true); + return d ? d->pdata : NULL; +} + +static void connector_write_request(WbDevice *d, WbRequest *r) { + Connector *con = d->pdata; + if (con->enable_presence) { + request_write_uchar(r, C_CONNECTOR_GET_PRESENCE); + request_write_uint16(r, con->presence_sampling_period); + con->enable_presence = false; + } + if (con->set_locked_state) { + request_write_uchar(r, con->is_locked ? C_CONNECTOR_LOCK : C_CONNECTOR_UNLOCK); + con->set_locked_state = false; + } +} + +static void connector_read_answer(WbDevice *d, WbRequest *r) { + Connector *con = d->pdata; + switch (request_read_uchar(r)) { + case C_CONNECTOR_GET_PRESENCE: + con->presence = request_read_int16(r); + break; + case C_CONFIGURE: + con->is_locked = request_read_uchar(r); + break; + default: + ROBOT_ASSERT(0); + } +} + +static void connector_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void connector_toggle_remote(WbDevice *d, WbRequest *r) { + Connector *con = d->pdata; + if (con->presence_sampling_period != 0) + con->enable_presence = true; + con->set_locked_state = true; +} + +// Exported functions + +void wb_connector_init(WbDevice *d) { + d->read_answer = connector_read_answer; + d->write_request = connector_write_request; + d->cleanup = connector_cleanup; + d->pdata = connector_create(); + d->toggle_remote = connector_toggle_remote; +} + +// Public functions (available from the user API) + +void wb_connector_enable_presence(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Connector *con = connector_get_struct(tag); + if (con) { + con->enable_presence = true; + con->presence_sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_connector_disable_presence(WbDeviceTag tag) { + const Connector *con = connector_get_struct(tag); + if (con) + wb_connector_enable_presence(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_connector_get_presence_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Connector *con = connector_get_struct(tag); + if (con) + sampling_period = con->presence_sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +void wb_connector_lock(WbDeviceTag tag) { + robot_mutex_lock(); + Connector *con = connector_get_struct(tag); + if (con) { + con->set_locked_state = true; + con->is_locked = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_connector_unlock(WbDeviceTag tag) { + robot_mutex_lock(); + Connector *con = connector_get_struct(tag); + if (con) { + con->set_locked_state = true; + con->is_locked = false; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +int wb_connector_get_presence(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const Connector *con = connector_get_struct(tag); + if (con) { + if (con->presence_sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_connector_enable_presence().\n", __FUNCTION__); + result = con->presence; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +bool wb_connector_is_locked(WbDeviceTag tag) { + bool result; + robot_mutex_lock(); + const Connector *con = connector_get_struct(tag); + if (con) + result = con->is_locked; + else { + result = false; + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + } + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/console.c b/src/controller/c/console.c new file mode 100644 index 0000000..e50965d --- /dev/null +++ b/src/controller/c/console.c @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "messages.h" +#include "request.h" +#include "robot_private.h" +#include "scheduler.h" + +#include + +void wb_console_print(const char *text, int stream) { + robot_console_print(text, stream); +} diff --git a/src/controller/c/default_robot_window.c b/src/controller/c/default_robot_window.c new file mode 100644 index 0000000..2b35250 --- /dev/null +++ b/src/controller/c/default_robot_window.c @@ -0,0 +1,895 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +// This module implements utility functions for managing the default HTML +// robot windows. These functions can be re-used for implementing specific +// user robot windows. +// Public functions are prefixed with wbu_default_robot_window_ +// This module is not yet complete: only a few devices are implemented. + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "base64.h" +#include "g_image.h" +#include "robot_private.h" + +#define MIN(x, y) (((x) < (y)) ? (x) : (y)) + +// Buffer management. + +static char *buffer = NULL; +static int buffer_size = 0; +static int images_max_width = -1; +static int images_max_height = -1; + +static void free_buffer() { + free(buffer); + buffer = NULL; +} + +static void create_buffer(int size) { + atexit(free_buffer); + buffer_size = size + 1; + buffer = malloc(buffer_size); +} + +static void throw_realloc_error() { + fprintf(stderr, "Error creating message to be sent to the robot window: not enough memory.\n"); + exit(EXIT_FAILURE); +} + +static void buffer_append(const char *string) { + if (string == NULL || string[0] == '\0') + return; + const int l = strlen(string); + if (buffer == NULL) { + create_buffer(l); + memcpy(buffer, string, buffer_size); + return; + } + buffer = realloc(buffer, buffer_size + l); + if (buffer == NULL) + throw_realloc_error(); + memcpy(&buffer[buffer_size - 1], string, l + 1); + buffer_size += l; +} + +static void buffer_append_escaped_string(const char *string) { + if (string == NULL || string[0] == '\0') + return; + const int string_length = strlen(string); + int i, count = 0; + for (i = 0; i < string_length; ++i) { + if (string[i] == '"') + count++; + } + + const int l = string_length + count * 5; + if (buffer == NULL) { + create_buffer(l); + if (count == 0) { + memcpy(buffer, string, buffer_size); + return; + } + } else { + buffer = realloc(buffer, buffer_size + l); + if (buffer == NULL) + throw_realloc_error(); + if (count == 0) { + memcpy(&buffer[buffer_size - 1], string, l + 1); + buffer_size += l; + return; + } + buffer_size += l; + } + + // copy and escape special characters + int j = buffer_size - l - 1; + for (i = 0; i < string_length; ++i) { + if (string[i] == '"') { + buffer[j++] = '&'; + buffer[j++] = 'q'; + buffer[j++] = 'u'; + buffer[j++] = 'o'; + buffer[j++] = 't'; + buffer[j++] = ';'; + } else + buffer[j++] = string[i]; + } +} + +static void buffer_append_double(double d) { + char buf[32]; + snprintf(buf, 32, "%.17g", d); // so that we don't loose any precision + buffer_append(buf); +} + +static void buffer_append_int(int i) { + char buf[32]; + snprintf(buf, 32, "%d", i); + buffer_append(buf); +} + +// Update structure management. + +static double update_period_by_type(WbNodeType type) { + switch (type) { + case WB_NODE_CAMERA: + case WB_NODE_LIDAR: + case WB_NODE_RANGE_FINDER: + return 0.1; // 100ms + default: + return 0.04; // 40ms + } +} + +static double number_of_components(WbDeviceTag tag) { + WbNodeType type = wb_device_get_node_type(tag); + switch (type) { + case WB_NODE_ALTIMETER: + case WB_NODE_DISTANCE_SENSOR: + case WB_NODE_POSITION_SENSOR: + case WB_NODE_LIGHT_SENSOR: + case WB_NODE_LINEAR_MOTOR: + case WB_NODE_ROTATIONAL_MOTOR: + case WB_NODE_VACUUM_GRIPPER: + return 1; + case WB_NODE_ACCELEROMETER: + case WB_NODE_COMPASS: + case WB_NODE_GPS: + case WB_NODE_GYRO: + case WB_NODE_INERTIAL_UNIT: + return 3; + case WB_NODE_TOUCH_SENSOR: + return wb_touch_sensor_get_type(tag) == WB_TOUCH_SENSOR_FORCE3D ? 3 : 1; + default: + return 0; + } +} + +struct UpdateElement { + double last_update; + + int n_values; + int n_components; // 1 for 1D value, 3 for 3D values, etc. + double **values; + double *times; +}; +static unsigned int number_of_update_elements = 0; +static struct UpdateElement **update_elements = NULL; + +static void ue_init(struct UpdateElement *ue, int n_components) { + ue->last_update = 0.0; + ue->n_values = 0; + ue->n_components = n_components; + ue->values = NULL; + ue->times = NULL; +} + +static void ue_append(struct UpdateElement *ue, double update_time, const double *value) { + if (value == NULL) + return; + + const int last_index = ue->n_values; + ue->n_values++; + if (ue->values == NULL) + ue->values = (double **)malloc(sizeof(double *)); + else + ue->values = (double **)realloc(ue->values, ue->n_values * sizeof(double *)); + if (ue->times == NULL) + ue->times = (double *)malloc(sizeof(double)); + else + ue->times = (double *)realloc(ue->times, ue->n_values * sizeof(double)); + ue->values[last_index] = malloc(ue->n_components * sizeof(double *)); + ue->times[last_index] = update_time; + int v; + for (v = 0; v < ue->n_components; ++v) + // cppcheck-suppress objectIndex + ue->values[last_index][v] = value[v]; +} + +static int ue_number_of_values(const struct UpdateElement *ue) { + return ue->n_values; +} + +static double *ue_value_at(struct UpdateElement *ue, int v) { + assert(v < ue->n_values); + return ue->values[v]; +} + +static double ue_time_at(struct UpdateElement *ue, int v) { + assert(v < ue->n_values); + return ue->times[v]; +} + +static void ue_clear(struct UpdateElement *ue) { + int v; + for (v = 0; v < ue_number_of_values(ue); ++v) + free(ue->values[v]); + free(ue->values); + free(ue->times); + ue->values = NULL; + ue->times = NULL; + ue->n_values = 0; +} + +static void ue_write_values(struct UpdateElement *ue) { + if (ue_number_of_values(ue) == 0) + return; + int v, c; + buffer_append("\"update\":["); + for (v = 0; v < ue_number_of_values(ue); ++v) { + if (v != 0) + buffer_append(","); + buffer_append("{"); + buffer_append("\"time\":"); + buffer_append_double(ue_time_at(ue, v)); + buffer_append(",\"value\":"); + if (ue->n_components == 1) { + double value = *ue_value_at(ue, v); + if (isinf(value)) { + if (value < 0) + buffer_append("\"-Inf\""); + else + buffer_append("\"Inf\""); + } else if (isnan(value)) + buffer_append("\"NaN\""); + else + buffer_append_double(value); + } else { + const double *values = ue_value_at(ue, v); + buffer_append("["); + for (c = 0; c < ue->n_components; ++c) { + if (c != 0) + buffer_append(","); + double value = values[c]; + if (isinf(value)) { + if (value < 0) + buffer_append("\"-Inf\""); + else + buffer_append("\"Inf\""); + } else if (isnan(value)) + buffer_append("\"NaN\""); + else + buffer_append_double(value); + } + buffer_append("]"); + } + buffer_append("}"); + } + buffer_append("]"); + ue_clear(ue); +} + +static void initialize_update_elements() { + const int number_of_devices = wb_robot_get_number_of_devices(); + if (update_elements) { + assert(number_of_devices > number_of_update_elements); + update_elements = realloc(update_elements, number_of_devices * sizeof(struct UpdateElement)); + if (!update_elements) { + fprintf(stderr, "Error reinitializing list of devices.\n"); + exit(EXIT_FAILURE); + } + } else + update_elements = malloc(number_of_devices * sizeof(struct UpdateElement)); + + int i; + for (i = number_of_update_elements; i < number_of_devices; ++i) { + WbDeviceTag tag = wb_robot_get_device_by_index(i); + struct UpdateElement *update_element = malloc(sizeof(struct UpdateElement)); + ue_init(update_element, number_of_components(tag)); + update_elements[i] = update_element; + } + number_of_update_elements = wb_robot_get_number_of_devices(); +} + +void default_robot_window_cleanup() { + int i; + for (i = 0; i < number_of_update_elements; ++i) + ue_clear(update_elements[i]); + free(update_elements); + update_elements = NULL; +} + +// Device configure functions. + +static void camera_configure(WbDeviceTag tag) { + buffer_append(",\"width\":"); + buffer_append_int(wb_camera_get_width(tag)); + buffer_append(",\"height\":"); + buffer_append_int(wb_camera_get_height(tag)); + buffer_append(",\"recognition\":"); + buffer_append_int(wb_camera_has_recognition(tag)); + buffer_append(",\"segmentation\":"); + buffer_append_int(wb_camera_recognition_has_segmentation(tag)); +} + +static void lidar_configure(WbDeviceTag tag) { + buffer_append(",\"width\":"); + buffer_append_int(wb_lidar_get_horizontal_resolution(tag)); + buffer_append(",\"height\":"); + buffer_append_int(wb_lidar_get_number_of_layers(tag)); +} + +static void range_finder_configure(WbDeviceTag tag) { + buffer_append(",\"width\":"); + buffer_append_int(wb_range_finder_get_width(tag)); + buffer_append(",\"height\":"); + buffer_append_int(wb_range_finder_get_height(tag)); +} + +static void distance_sensor_configure(WbDeviceTag tag) { + buffer_append(",\"sensorType\":\""); + switch (wb_distance_sensor_get_type(tag)) { + case WB_DISTANCE_SENSOR_GENERIC: + buffer_append("generic"); + break; + case WB_DISTANCE_SENSOR_INFRA_RED: + buffer_append("infra-red"); + break; + case WB_DISTANCE_SENSOR_LASER: + buffer_append("laser"); + break; + case WB_DISTANCE_SENSOR_SONAR: + buffer_append("sonar"); + break; + default: + buffer_append("unknown"); + break; + } + buffer_append("\",\"minValue\":"); + buffer_append_double(wb_distance_sensor_get_min_value(tag)); + buffer_append(",\"maxValue\":"); + buffer_append_double(wb_distance_sensor_get_max_value(tag)); + buffer_append(",\"aperture\":"); + buffer_append_double(wb_distance_sensor_get_aperture(tag)); +} + +static void motor_configure(WbDeviceTag tag) { + buffer_append(",\"minPosition\":"); + buffer_append_double(wb_motor_get_min_position(tag)); + buffer_append(",\"maxPosition\":"); + buffer_append_double(wb_motor_get_max_position(tag)); + buffer_append(",\"maxVelocity\":"); + buffer_append_double(wb_motor_get_max_velocity(tag)); + switch (wb_device_get_node_type(tag)) { + case WB_NODE_ROTATIONAL_MOTOR: + buffer_append(",\"maxTorque\":"); + buffer_append_double(wb_motor_get_max_torque(tag)); + break; + case WB_NODE_LINEAR_MOTOR: + buffer_append(",\"maxForce\":"); + buffer_append_double(wb_motor_get_max_force(tag)); + break; + default: + assert(0); + } +} + +static void radar_configure(WbDeviceTag tag) { + buffer_append(",\"fieldOfView\":"); + buffer_append_double(wb_radar_get_horizontal_fov(tag)); + buffer_append(",\"minRange\":"); + buffer_append_double(wb_radar_get_min_range(tag)); + buffer_append(",\"maxRange\":"); + buffer_append_double(wb_radar_get_max_range(tag)); +} + +static void touch_sensor_configure(WbDeviceTag tag) { + buffer_append(",\"sensorType\":\""); + switch (wb_touch_sensor_get_type(tag)) { + case WB_TOUCH_SENSOR_BUMPER: + buffer_append("bumper"); + break; + case WB_TOUCH_SENSOR_FORCE: + buffer_append("force"); + break; + case WB_TOUCH_SENSOR_FORCE3D: + buffer_append("force-3d"); + break; + default: + assert(0); + } + buffer_append("\""); +} + +void wbu_default_robot_window_configure() { + buffer_append("configure {\"name\":\""); + buffer_append_escaped_string(wb_robot_get_name()); + buffer_append("\",\"model\":\""); + buffer_append_escaped_string(wb_robot_get_model()); + buffer_append("\",\"basicTimeStep\":"); + buffer_append_double(0.001 * wb_robot_get_basic_time_step()); + int n = wb_robot_get_number_of_devices(); + if (n) { + buffer_append(",\"devices\":["); + int i; + for (i = 0; i < n; ++i) { + WbDeviceTag tag = wb_robot_get_device_by_index(i); + if (i > 0) + buffer_append(","); + buffer_append("{\"type\":\""); + WbNodeType type = wb_device_get_node_type(tag); + buffer_append_escaped_string(wb_node_get_name(type)); + buffer_append("\",\"name\":\""); + buffer_append_escaped_string(wb_device_get_name(tag)); + buffer_append("\",\"model\":\""); + buffer_append_escaped_string(wb_device_get_model(tag)); + buffer_append("\""); + switch (type) { + case WB_NODE_CAMERA: + camera_configure(tag); + break; + case WB_NODE_DISTANCE_SENSOR: + distance_sensor_configure(tag); + break; + case WB_NODE_LIDAR: + lidar_configure(tag); + break; + case WB_NODE_RADAR: + radar_configure(tag); + break; + case WB_NODE_RANGE_FINDER: + range_finder_configure(tag); + break; + case WB_NODE_ROTATIONAL_MOTOR: + case WB_NODE_LINEAR_MOTOR: + motor_configure(tag); + break; + case WB_NODE_TOUCH_SENSOR: + touch_sensor_configure(tag); + break; + default: + break; + } + buffer_append("}"); + } + buffer_append("]"); + } + buffer_append("}"); + wb_robot_wwi_send_text(buffer); + free_buffer(); +} + +static void append_rescaled_image_to_buffer_and_free_data(GImage *img, int new_width, int new_height, float max_range) { + unsigned char *jpeg_data = NULL; + char *base64_data = NULL; + + // 1. Downscale image. + int success = g_image_downscale(img, new_width, new_height, max_range); + if (img->data == NULL || success != 0) + assert(0); + else { + // 2. Convert to JPEG. + unsigned long jpeg_data_size = 0; + success = g_image_save_to_jpeg_buffer(img, &jpeg_data, &jpeg_data_size, 100); + if (jpeg_data == NULL || success != 0) + assert(0); + else { + // 3. Convert to base64. + size_t output_length; + base64_data = base64_encode(jpeg_data, jpeg_data_size, &output_length); + if (base64_data == NULL) + assert(0); + else { + // 4. Send to robot window. + buffer_append("\"image\":\"data:image/jpg;base64,"); + buffer_append(base64_data); + buffer_append("\""); + } + } + } + + // 5. Cleanup + free(jpeg_data); + free(base64_data); + free(img->data); + img->data = NULL; +} + +// Device update functions. + +static void camera_update(WbDeviceTag tag) { + if (wb_camera_get_sampling_period(tag) <= 0) + return; + + if (wb_camera_has_recognition(tag)) { + buffer_append("\"recognitionEnabled\":"); + buffer_append(wb_camera_recognition_get_sampling_period(tag) > 0 ? "true" : "false"); + if (wb_camera_recognition_has_segmentation(tag)) { + buffer_append(",\"segmentationEnabled\":"); + buffer_append(wb_camera_recognition_is_segmentation_enabled(tag) ? "true" : "false"); + } + buffer_append(","); + } + + const unsigned char *image = wb_camera_get_image(tag); + if (!image) + return; + + int camera_width = wb_camera_get_width(tag); + int camera_height = wb_camera_get_height(tag); + + int new_width = camera_width; + int new_height = camera_height; + + if (images_max_width > 0) { + if (new_width > images_max_width) { + double ratio = (double)images_max_width / new_width; + new_height = ratio * new_height; + new_width = ratio * new_width; + } + if (new_height > images_max_height) { + double ratio = (double)images_max_height / new_height; + new_height = ratio * new_height; + new_width = ratio * new_width; + } + } + + GImage img; + img.width = camera_width; + img.height = camera_height; + img.data_format = G_IMAGE_DATA_FORMAT_BGRA; + img.float_data = NULL; + img.data = (unsigned char *)image; + img.failed = 0; + img.flipped = 0; + + append_rescaled_image_to_buffer_and_free_data(&img, new_width, new_height, 0.0f); +} + +static void lidar_update(WbDeviceTag tag) { + buffer_append("\"cloudPointEnabled\":"); + buffer_append(wb_lidar_is_point_cloud_enabled(tag) ? "true" : "false"); + + if (wb_lidar_get_sampling_period(tag) <= 0) + return; + + const float *image = wb_lidar_get_range_image(tag); + if (!image) + return; + + buffer_append(","); + + int lidar_width = wb_lidar_get_horizontal_resolution(tag); + int lidar_height = wb_lidar_get_number_of_layers(tag); + int new_width = lidar_width; + int new_height = lidar_height; + + if (images_max_width > 0) { + // assumption: no need to rescale along the height, because nLayers is small enough. + assert(lidar_height < images_max_height); + new_width = MIN(lidar_width, images_max_width); + } + + GImage img; + img.width = lidar_width; + img.height = lidar_height; + img.data_format = G_IMAGE_DATA_FORMAT_F; + img.float_data = (float *)image; + img.data = NULL; + img.failed = 0; + img.flipped = 0; + + append_rescaled_image_to_buffer_and_free_data(&img, new_width, new_height, wb_lidar_get_max_range(tag)); +} + +static void range_finder_update(WbDeviceTag tag) { + if (wb_range_finder_get_sampling_period(tag) <= 0) + return; + + const float *image = wb_range_finder_get_range_image(tag); + if (!image) + return; + + int range_finder_width = wb_range_finder_get_width(tag); + int range_finder_height = wb_range_finder_get_height(tag); + + int new_width = range_finder_width; + int new_height = range_finder_height; + + if (images_max_width > 0) { + if (new_width > images_max_width) { + double ratio = (double)images_max_width / new_width; + new_height = ratio * new_height; + new_width = ratio * new_width; + } + if (new_height > images_max_height) { + double ratio = (double)images_max_height / new_height; + new_height = ratio * new_height; + new_width = ratio * new_width; + } + } + + GImage img; + img.width = range_finder_width; + img.height = range_finder_height; + img.data_format = G_IMAGE_DATA_FORMAT_F; + img.float_data = (float *)image; + img.data = NULL; + img.failed = 0; + img.flipped = 0; + + append_rescaled_image_to_buffer_and_free_data(&img, new_width, new_height, wb_range_finder_get_max_range(tag)); +} + +static void accelerometer_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_accelerometer_get_sampling_period(tag) <= 0) + return; + const double *values = wb_accelerometer_get_values(tag); + ue_append(ue, update_time, values); +} + +static void altimeter_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_altimeter_get_sampling_period(tag) <= 0) + return; + const double value = wb_altimeter_get_value(tag); + ue_append(ue, update_time, &value); +} + +static void compass_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_compass_get_sampling_period(tag) <= 0) + return; + const double *values = wb_compass_get_values(tag); + ue_append(ue, update_time, values); +} + +static void distance_sensor_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_distance_sensor_get_sampling_period(tag) <= 0) + return; + double value = wb_distance_sensor_get_value(tag); + ue_append(ue, update_time, &value); +} + +static void gps_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_gps_get_sampling_period(tag) <= 0) + return; + const double *values = wb_gps_get_values(tag); + ue_append(ue, update_time, values); +} + +static void gyro_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_gyro_get_sampling_period(tag) <= 0) + return; + const double *values = wb_gyro_get_values(tag); + ue_append(ue, update_time, values); +} + +static void inertial_unit_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_inertial_unit_get_sampling_period(tag) <= 0) + return; + const double *values = wb_inertial_unit_get_roll_pitch_yaw(tag); + ue_append(ue, update_time, values); +} + +static void light_sensor_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_light_sensor_get_sampling_period(tag) <= 0) + return; + double value = wb_light_sensor_get_value(tag); + ue_append(ue, update_time, &value); +} + +static void motor_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + double value = wb_motor_get_target_position(tag); + ue_append(ue, update_time, &value); +} + +static void position_sensor_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_position_sensor_get_sampling_period(tag) <= 0) + return; + double value = wb_position_sensor_get_value(tag); + ue_append(ue, update_time, &value); +} + +static void radar_update(WbDeviceTag tag) { + if (wb_radar_get_sampling_period(tag) <= 0) + return; + buffer_append("\"targets\":["); + int t; + for (t = 0; t < wb_radar_get_number_of_targets(tag); ++t) { + if (t != 0) + buffer_append(","); + buffer_append("{\"distance\":"); + buffer_append_double(wb_radar_get_targets(tag)[t].distance); + buffer_append(",\"azimuth\":"); + buffer_append_double(wb_radar_get_targets(tag)[t].azimuth); + buffer_append("}"); + } + buffer_append("]"); +} + +static void touch_sensor_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_touch_sensor_get_sampling_period(tag) <= 0) + return; + if (wb_touch_sensor_get_type(tag) == WB_TOUCH_SENSOR_FORCE3D) { + const double *values = wb_touch_sensor_get_values(tag); + ue_append(ue, update_time, values); + } else { + double value = wb_touch_sensor_get_value(tag); + ue_append(ue, update_time, &value); + } +} + +static void vacuum_gripper_collect_value(WbDeviceTag tag, struct UpdateElement *ue, double update_time) { + if (wb_vacuum_gripper_get_presence_sampling_period(tag) <= 0) + return; + const double value = wb_vacuum_gripper_get_presence(tag) ? 1.0 : 0.0; + ue_append(ue, update_time, &value); +} + +static void vacuum_gripper_update(WbDeviceTag tag, struct UpdateElement *ue) { + if (wb_vacuum_gripper_get_presence_sampling_period(tag) > 0) { + ue_write_values(ue); + buffer_append(","); + } + buffer_append("\"vacuumGripperOn\":"); + buffer_append(wb_vacuum_gripper_is_on(tag) ? "true" : "false"); +} + +void wbu_default_robot_window_update() { + if (buffer != NULL) + return; // prevent to mix 2 updates. + + const int n = wb_robot_get_number_of_devices(); + if (update_elements == NULL || n > number_of_update_elements) { + const bool new_devices = update_elements != NULL; + initialize_update_elements(); + if (new_devices) + wbu_default_robot_window_configure(); + } + + buffer_append("update {"); + const double simulated_time = wb_robot_get_time(); + buffer_append("\"time\":"); + buffer_append_double(simulated_time); + if (n) { + buffer_append(",\"devices\":{"); + int i; + int updated_device = 0; + for (i = 0; i < n; ++i) { + const WbDeviceTag tag = wb_robot_get_device_by_index(i); + const WbNodeType type = wb_device_get_node_type(tag); + struct UpdateElement *update_element = update_elements[i]; + + // store values to be sent later if required. + switch (type) { + case WB_NODE_ROTATIONAL_MOTOR: + case WB_NODE_LINEAR_MOTOR: + motor_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_ACCELEROMETER: + accelerometer_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_ALTIMETER: + altimeter_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_COMPASS: + compass_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_DISTANCE_SENSOR: + distance_sensor_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_GPS: + gps_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_GYRO: + gyro_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_INERTIAL_UNIT: + inertial_unit_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_LIGHT_SENSOR: + light_sensor_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_POSITION_SENSOR: + position_sensor_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_TOUCH_SENSOR: + touch_sensor_collect_value(tag, update_element, simulated_time); + break; + case WB_NODE_VACUUM_GRIPPER: + vacuum_gripper_collect_value(tag, update_element, simulated_time); + break; + default: + break; + } + + if (robot_get_simulation_mode() == WB_SUPERVISOR_SIMULATION_MODE_PAUSE || + update_element->last_update + update_period_by_type(type) < simulated_time) { + // send the stored values if any. + update_element->last_update = simulated_time; + if (updated_device > 0) + buffer_append(","); + ++updated_device; + buffer_append("\""); + buffer_append_escaped_string(wb_device_get_name(tag)); + buffer_append("\":{"); + switch (type) { + case WB_NODE_ACCELEROMETER: + case WB_NODE_ALTIMETER: + case WB_NODE_COMPASS: + case WB_NODE_DISTANCE_SENSOR: + case WB_NODE_GPS: + case WB_NODE_GYRO: + case WB_NODE_INERTIAL_UNIT: + case WB_NODE_LIGHT_SENSOR: + case WB_NODE_LINEAR_MOTOR: + case WB_NODE_POSITION_SENSOR: + case WB_NODE_ROTATIONAL_MOTOR: + case WB_NODE_TOUCH_SENSOR: + ue_write_values(update_element); + break; + case WB_NODE_CAMERA: + camera_update(tag); + break; + case WB_NODE_LIDAR: + lidar_update(tag); + break; + case WB_NODE_RADAR: + radar_update(tag); + break; + case WB_NODE_RANGE_FINDER: + range_finder_update(tag); + break; + case WB_NODE_VACUUM_GRIPPER: + vacuum_gripper_update(tag, update_element); + break; + default: + break; + } + buffer_append("}"); + } + } + buffer_append("}"); + } + buffer_append("}"); + wb_robot_wwi_send_text(buffer); + free_buffer(); +} + +void wbu_default_robot_window_set_images_max_size(int max_width, int max_height) { + assert(max_width > 0); + assert(max_height > 0); + images_max_width = max_width; + images_max_height = max_height; +} diff --git a/src/controller/c/default_robot_window_private.h b/src/controller/c/default_robot_window_private.h new file mode 100644 index 0000000..ddd7466 --- /dev/null +++ b/src/controller/c/default_robot_window_private.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef DEFAULT_ROBOT_WINDOW_PRIVATE_H +#define DEFAULT_ROBOT_WINDOW_PRIVATE_H + +void default_robot_window_cleanup(); + +#endif diff --git a/src/controller/c/device.c b/src/controller/c/device.c new file mode 100644 index 0000000..de76365 --- /dev/null +++ b/src/controller/c/device.c @@ -0,0 +1,171 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "robot_private.h" + +const char *wb_device_get_name(WbDeviceTag dt) { + return robot_get_device_name(dt); +} + +const char *wb_device_get_model(WbDeviceTag dt) { + return robot_get_device_model(dt); +} + +WbNodeType wb_device_get_node_type(WbDeviceTag dt) { + return robot_get_device_type(dt); +} + +void wb_device_cleanup(WbDevice *d) { + free(d->name); + if (d->cleanup) + d->cleanup(d); + free(d); +} + +extern void wb_accelerometer_init(WbDevice *); +extern void wb_altimeter_init(WbDevice *); +extern void wb_brake_init(WbDevice *); +extern void wb_camera_init(WbDevice *); +extern void wb_compass_init(WbDevice *); +extern void wb_connector_init(WbDevice *); +extern void wb_display_init(WbDevice *); +extern void wb_distance_sensor_init(WbDevice *); +extern void wb_emitter_init(WbDevice *); +extern void wb_gps_init(WbDevice *); +extern void wb_gyro_init(WbDevice *); +extern void wb_inertial_unit_init(WbDevice *); +extern void wb_led_init(WbDevice *); +extern void wb_lidar_init(WbDevice *); +extern void wb_light_sensor_init(WbDevice *); +extern void wb_microphone_init(WbDevice *); +extern void wb_motor_init(WbDevice *); +extern void wb_pen_init(WbDevice *); +extern void wb_position_sensor_init(WbDevice *); +extern void wb_radar_init(WbDevice *); +extern void wb_radio_init(WbDevice *); +extern void wb_range_finder_init(WbDevice *); +extern void wb_receiver_init(WbDevice *); +extern void wb_skin_init(WbDevice *); +extern void wb_speaker_init(WbDevice *); +extern void wb_touch_sensor_init(WbDevice *); +extern void wb_vacuum_gripper_init(WbDevice *); + +void wb_device_init(WbDevice *d) { + d->toggle_remote = NULL; + + switch (d->node) { + case WB_NODE_ACCELEROMETER: + wb_accelerometer_init(d); + break; + case WB_NODE_ALTIMETER: + wb_altimeter_init(d); + break; + case WB_NODE_BRAKE: + wb_brake_init(d); + break; + case WB_NODE_CAMERA: + wb_camera_init(d); + break; + case WB_NODE_COMPASS: + wb_compass_init(d); + break; + case WB_NODE_CONNECTOR: + wb_connector_init(d); + break; + case WB_NODE_DISPLAY: + wb_display_init(d); + break; + case WB_NODE_DISTANCE_SENSOR: + wb_distance_sensor_init(d); + break; + case WB_NODE_EMITTER: + wb_emitter_init(d); + break; + case WB_NODE_GPS: + wb_gps_init(d); + break; + case WB_NODE_GYRO: + wb_gyro_init(d); + break; + case WB_NODE_INERTIAL_UNIT: + wb_inertial_unit_init(d); + break; + case WB_NODE_LED: + wb_led_init(d); + break; + case WB_NODE_LIDAR: + wb_lidar_init(d); + break; + case WB_NODE_LIGHT_SENSOR: + wb_light_sensor_init(d); + break; + case WB_NODE_LINEAR_MOTOR: + wb_motor_init(d); + break; + case WB_NODE_MICROPHONE: + wb_microphone_init(d); + break; + case WB_NODE_PEN: + wb_pen_init(d); + break; + case WB_NODE_POSITION_SENSOR: + wb_position_sensor_init(d); + break; + case WB_NODE_RADAR: + wb_radar_init(d); + break; + case WB_NODE_RADIO: + wb_radio_init(d); + break; + case WB_NODE_RANGE_FINDER: + wb_range_finder_init(d); + break; + case WB_NODE_RECEIVER: + wb_receiver_init(d); + break; + case WB_NODE_ROTATIONAL_MOTOR: + wb_motor_init(d); + break; + case WB_NODE_SKIN: + wb_skin_init(d); + break; + case WB_NODE_SPEAKER: + wb_speaker_init(d); + break; + case WB_NODE_TOUCH_SENSOR: + wb_touch_sensor_init(d); + break; + case WB_NODE_VACUUM_GRIPPER: + wb_vacuum_gripper_init(d); + break; + default: + fprintf(stderr, "%s(): node not handled\n", __FUNCTION__); + break; + } +} + +// obsolete function kept for backward compatibility + +WbNodeType wb_device_get_type(WbDeviceTag dt) { + return wb_device_get_node_type(dt); +} diff --git a/src/controller/c/device_private.h b/src/controller/c/device_private.h new file mode 100644 index 0000000..94437e4 --- /dev/null +++ b/src/controller/c/device_private.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef DEVICE_PRIVATE_H +#define DEVICE_PRIVATE_H + +#include +#include "request.h" + +#ifndef NAN +#define NAN (0.0 / 0.0) +#endif + +typedef struct _WbDevice WbDevice; + +struct _WbDevice { + WbNodeType node; + char *name; + char *model; + void *pdata; + void (*read_answer)(WbDevice *, WbRequest *); + void (*write_request)(WbDevice *, WbRequest *); + void (*cleanup)(WbDevice *); + void (*toggle_remote)(WbDevice *, WbRequest *); +}; + +void wb_device_cleanup(WbDevice *); +void wb_device_init(WbDevice *); + +#endif // DEVICE_PRIVATE_H diff --git a/src/controller/c/display.c b/src/controller/c/display.c new file mode 100644 index 0000000..396f466 --- /dev/null +++ b/src/controller/c/display.c @@ -0,0 +1,887 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "display_private.h" +#include "g_image.h" +#include "messages.h" +#include "remote_control_private.h" //required to forward the display state from Webots to the remote_control +#include "robot_private.h" + +typedef struct WbImageStructPrivate { + int id; + WbDeviceTag device_tag; +} WbImageStruct; + +// WbImageRef handle message +typedef struct { + int id; + int x; + int y; + int width; + int height; + int format; + bool blend; + void *image; +} DisplayImageMessage; + +// a drawing primitive message +typedef struct { + bool pfill; + int *px; + int *py; + int psize; + char *ptxt; +} DisplayPrimitiveMessage; + +typedef struct { + char *name; + int size; + bool anti_aliasing; +} DisplayFontProperties; + +// a drawing property message +typedef struct { + union { + int color; + double alpha; + double opacity; + DisplayFontProperties font_properties; + }; +} DisplayPropertyMessage; + +typedef struct _SaveOrder_ { + int id; + char *filename; + unsigned char type; + struct _SaveOrder_ *next; +} SaveOrder; + +// a message contains either a drawing primitive +// or either a drawing property +typedef struct _DisplayMessage_ { + int message; + union { + DisplayPrimitiveMessage *primitive_message; + DisplayPropertyMessage *property_message; + DisplayImageMessage *image_message; + }; + struct _DisplayMessage_ *next; +} DisplayMessage; + +typedef struct { + int width; + int height; + int image_next_free_id; + DisplayMessage *messages_head; // list of messages to send + DisplayMessage *messages_tail; + SaveOrder *save_orders; // list of images which will be saved + bool hasBeenUsed; + WbDeviceTag camera_to_attach; + bool is_camera_attached; + bool camera_to_detach; +} Display; + +static Display *wb_display_create(int width, int height) { + Display *display = malloc(sizeof(Display)); + display->width = width; + display->height = height; + display->image_next_free_id = 1; // 0 is reserved for current display content + display->messages_head = NULL; + display->messages_tail = NULL; + display->save_orders = NULL; + display->hasBeenUsed = false; + display->camera_to_attach = 0; + display->is_camera_attached = false; + display->camera_to_detach = false; + return display; +} + +static inline void message_enqueue(Display *d, DisplayMessage *new) { + new->next = NULL; + if (d->messages_head == NULL) + d->messages_head = new; + else + d->messages_tail->next = new; + d->messages_tail = new; +} + +static Display *wb_display_get_struct(WbDeviceTag tag) { + WbDevice *d = robot_get_device_with_node(tag, WB_NODE_DISPLAY, true); + return d ? d->pdata : NULL; +} + +int display_get_channel_number(int pixel_format) { + switch (pixel_format) { + case WB_IMAGE_RGB: + return 3; + case WB_IMAGE_RGBA: + case WB_IMAGE_ARGB: + case WB_IMAGE_BGRA: + case WB_IMAGE_ABGR: + return 4; + default: + assert(0); + } + return 0; +} + +static bool save_image(Display *d, int id, int width, int height, unsigned char *im) { + SaveOrder *order = d->save_orders; + if (order && order->id == id) { + GImage *i = (GImage *)malloc(sizeof(GImage)); + i->width = width; + i->height = height; + i->failed = 0; + i->flipped = 0; + i->data_format = G_IMAGE_DATA_FORMAT_BGRA; + i->data = im; + g_image_save(i, order->filename, 100); + free(i); + free(order->filename); + d->save_orders = d->save_orders->next; + free(order); + return true; + } else + return false; +} + +static void wb_display_read_answer(WbDevice *d, WbRequest *r) { + int width, height, id; + unsigned char *im; + switch (request_read_uchar(r)) { + case C_CONFIGURE: + width = request_read_uint16(r); + height = request_read_uint16(r); + d->pdata = wb_display_create(width, height); + break; + case C_DISPLAY_IMAGE_SAVE: + id = request_read_int32(r); + width = request_read_int32(r); + height = request_read_int32(r); + im = request_read_data(r, 4 * width * height); + save_image(d->pdata, id, width, height, im); + break; + case C_DISPLAY_IMAGE_GET_ALL: + r->pointer--; // unread command + remote_control_handle_one_message(r, robot_get_device_tag(d)); + break; + default: + ROBOT_ASSERT(0); + break; + } +} + +static void wb_display_write_request(WbDevice *d, WbRequest *r) { + Display *display = d->pdata; + DisplayMessage *current_message = display->messages_head; + + if (display->camera_to_attach != 0) { + request_write_uchar(r, C_DISPLAY_ATTACH_CAMERA); + request_write_uint16(r, display->camera_to_attach); + display->camera_to_attach = 0; + } else if (display->camera_to_detach) { + request_write_uchar(r, C_DISPLAY_DETACH_CAMERA); + display->camera_to_detach = false; + } + + // for each element of the request list, write a request + while (current_message) { + display->hasBeenUsed = true; + int message = current_message->message; + request_write_uchar(r, message); + if (message == C_DISPLAY_SET_COLOR) { + request_write_int32(r, current_message->property_message->color); + free(current_message->property_message); + } else if (message == C_DISPLAY_SET_ALPHA) { + request_write_uchar(r, (unsigned char)(current_message->property_message->alpha * 255)); + free(current_message->property_message); + } else if (message == C_DISPLAY_SET_OPACITY) { + request_write_uchar(r, (unsigned char)(current_message->property_message->opacity * 255)); + free(current_message->property_message); + } else if (message == C_DISPLAY_SET_FONT) { + request_write_uint32(r, current_message->property_message->font_properties.size); + request_write_uchar(r, current_message->property_message->font_properties.anti_aliasing ? 1 : 0); + request_write_uint16(r, strlen(current_message->property_message->font_properties.name) + 1); + request_write_string(r, current_message->property_message->font_properties.name); + free(current_message->property_message->font_properties.name); + free(current_message->property_message); + } else if (message & 0x20) { // a drawing primitive + request_write_int16(r, current_message->primitive_message->psize); + request_write_data(r, current_message->primitive_message->px, current_message->primitive_message->psize * sizeof(int)); + request_write_data(r, current_message->primitive_message->py, current_message->primitive_message->psize * sizeof(int)); + if (message == C_DISPLAY_DRAW_TEXT) + request_write_string(r, current_message->primitive_message->ptxt); + else if (message == C_DISPLAY_DRAW_RECTANGLE || message == C_DISPLAY_DRAW_OVAL || message == C_DISPLAY_DRAW_POLYGON) + request_write_uchar(r, current_message->primitive_message->pfill ? 1 : 0); + free(current_message->primitive_message->px); + free(current_message->primitive_message->py); + free(current_message->primitive_message->ptxt); + free(current_message->primitive_message); + } else if (message & 0x40) { // WbImageRef handle message + request_write_int32(r, current_message->image_message->id); + if (message == C_DISPLAY_IMAGE_PASTE || message == C_DISPLAY_IMAGE_COPY) { + request_write_int16(r, current_message->image_message->x); + request_write_int16(r, current_message->image_message->y); + } + if (message == C_DISPLAY_IMAGE_PASTE) + request_write_uchar(r, current_message->image_message->blend ? 1 : 0); + if (message == C_DISPLAY_IMAGE_COPY || message == C_DISPLAY_IMAGE_LOAD) { + request_write_int16(r, current_message->image_message->width); + request_write_int16(r, current_message->image_message->height); + } + if (message == C_DISPLAY_IMAGE_LOAD) { + request_write_uchar(r, current_message->image_message->format); + request_write_data(r, current_message->image_message->image, + current_message->image_message->width * current_message->image_message->height * + display_get_channel_number(current_message->image_message->format)); + free(current_message->image_message->image); + } + free(current_message->image_message); + } + DisplayMessage *tmp = current_message->next; + free(current_message); + current_message = tmp; + } + display->messages_head = NULL; + display->messages_tail = NULL; +} + +static void wb_display_cleanup(WbDevice *d) { + Display *dp = d->pdata; + DisplayMessage *current_message; + while (dp->messages_head) { + current_message = dp->messages_head->next; + if (dp->messages_head->message & 0x20) { + free(dp->messages_head->primitive_message->px); + free(dp->messages_head->primitive_message->py); + free(dp->messages_head->primitive_message->ptxt); + free(dp->messages_head->primitive_message); + } else if (dp->messages_head->message & 0x40) { + if (dp->messages_head->message == C_DISPLAY_IMAGE_LOAD) + free(dp->messages_head->image_message->image); + free(dp->messages_head->image_message); + } else { + free(dp->messages_head->property_message); + } + free(dp->messages_head); + dp->messages_head = current_message; + } +} + +// add a drawing primitive request into Display's messages +static void wb_display_draw_primitive(WbDeviceTag tag, int primitive, const int *x, const int *y, int size, bool fill, + const char *txt) { + assert(x && y && size > 0); + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayPrimitiveMessage *p = (DisplayPrimitiveMessage *)malloc(sizeof(DisplayPrimitiveMessage)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (d && m && p) { + m->message = primitive; + p->pfill = fill; + p->psize = size; + p->px = (int *)malloc(size * sizeof(int)); + p->py = (int *)malloc(size * sizeof(int)); + memcpy(p->px, x, size * sizeof(int)); + memcpy(p->py, y, size * sizeof(int)); + if (txt) { + p->ptxt = (char *)malloc(strlen(txt) + 1); + if (p->ptxt) + strcpy(p->ptxt, txt); + } else + p->ptxt = NULL; + m->primitive_message = p; + message_enqueue(d, m); + } else { + free(m); + free(p); + } + robot_mutex_unlock(); +} + +// add a drawing property request into Display's messages +static void wb_display_set_property(WbDeviceTag tag, int primitive, void *data, void *font_size, void *anti_aliasing) { + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayPropertyMessage *p = (DisplayPropertyMessage *)malloc(sizeof(DisplayPropertyMessage)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (d && m && p) { + m->message = primitive; + switch (primitive) { + case C_DISPLAY_SET_COLOR: + p->color = *((int *)data); + break; + case C_DISPLAY_SET_ALPHA: + p->alpha = *((double *)data); + break; + case C_DISPLAY_SET_OPACITY: + p->opacity = *((double *)data); + break; + case C_DISPLAY_SET_FONT: { + const int size = strlen((char *)data) + 1; + p->font_properties.name = malloc(size); + memcpy(p->font_properties.name, (char *)data, size); + p->font_properties.size = *((int *)font_size); + p->font_properties.anti_aliasing = *((bool *)anti_aliasing); + break; + } + } + m->property_message = p; + message_enqueue(d, m); + } else { + free(m); + free(p); + } + robot_mutex_unlock(); +} + +static void display_toggle_remote(WbDevice *d, WbRequest *r) { + const Display *display = d->pdata; + if (display->hasBeenUsed) + request_write_uchar(r, C_DISPLAY_IMAGE_GET_ALL); +} + +void wbr_display_save_image(WbDeviceTag tag, int id, int width, int height, unsigned char *image) { + Display *d = wb_display_get_struct(tag); + if (d) { + if (!save_image(d, id, width, height, image)) + fprintf(stderr, "%s(): wrong id.\n", __FUNCTION__); + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Protected functions (exported to device.cc) + +void wb_display_init(WbDevice *d) { + d->pdata = NULL; + d->write_request = wb_display_write_request; + d->read_answer = wb_display_read_answer; + d->cleanup = wb_display_cleanup; + d->toggle_remote = display_toggle_remote; +} + +// Public function available from the user API + +int wb_display_get_height(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const Display *d = wb_display_get_struct(tag); + if (d) + result = d->height; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +int wb_display_get_width(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const Display *d = wb_display_get_struct(tag); + if (d) + result = d->width; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_display_set_color(WbDeviceTag tag, int color) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (color > 0xFFFFFF || color < 0) { + fprintf(stderr, "Error: %s(): 'color' argument out of bounds.\n", __FUNCTION__); + return; + } + wb_display_set_property(tag, C_DISPLAY_SET_COLOR, &color, NULL, NULL); +} + +void wb_display_set_alpha(WbDeviceTag tag, double alpha) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (alpha > 1.0 || alpha < 0.0) { + fprintf(stderr, "Error: %s(): 'alpha' argument out of bounds.\n", __FUNCTION__); + return; + } + wb_display_set_property(tag, C_DISPLAY_SET_ALPHA, &alpha, NULL, NULL); +} + +void wb_display_set_opacity(WbDeviceTag tag, double opacity) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (opacity > 1.0 || opacity < 0.0) { + fprintf(stderr, "Error: %s(): 'opacity' argument out of bounds.\n", __FUNCTION__); + return; + } + wb_display_set_property(tag, C_DISPLAY_SET_OPACITY, &opacity, NULL, NULL); +} + +void wb_display_set_font(WbDeviceTag tag, const char *font, int size, bool anti_aliasing) { + if (size <= 0) { + fprintf(stderr, "Error: %s(): 'size' argument is negative or null.\n", __FUNCTION__); + return; + } + robot_mutex_lock(); + const Display *display = wb_display_get_struct(tag); + if (!display) { + fprintf(stderr, "Error: %s(): invalid display.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + robot_mutex_unlock(); + wb_display_set_property(tag, C_DISPLAY_SET_FONT, (char *)font, &size, &anti_aliasing); +} + +void wb_display_attach_camera(WbDeviceTag tag, WbDeviceTag camera_tag) { + robot_mutex_lock(); + Display *display = wb_display_get_struct(tag); + const WbDevice *camera = robot_get_device_with_node(camera_tag, WB_NODE_CAMERA, true); + if (!display) { + fprintf(stderr, "Error: %s(): invalid display.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!camera) { + fprintf(stderr, "Error: %s(): invalid camera.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (display->is_camera_attached) { + fprintf(stderr, "Error: %s(): a camera is already attached to the display.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + display->camera_to_attach = camera_tag; + display->is_camera_attached = true; + display->camera_to_detach = false; + robot_mutex_unlock(); +} + +void wb_display_detach_camera(WbDeviceTag tag) { + robot_mutex_lock(); + Display *display = wb_display_get_struct(tag); + if (display == NULL) { + fprintf(stderr, "Error: %s(): invalid display.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + if (!display->is_camera_attached) { + fprintf(stderr, "Error: %s(): no camera to detach.\n", __FUNCTION__); + robot_mutex_unlock(); + return; + } + display->camera_to_attach = 0; + display->is_camera_attached = false; + display->camera_to_detach = true; + robot_mutex_unlock(); +} + +void wb_display_draw_pixel(WbDeviceTag tag, int x, int y) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + + const int px[] = {x}; + const int py[] = {y}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_PIXEL, px, py, 1, false, NULL); +} + +void wb_display_draw_line(WbDeviceTag tag, int x1, int y1, int x2, int y2) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + const int px[] = {x1, x2}; + const int py[] = {y1, y2}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_LINE, px, py, 2, false, NULL); +} + +void wb_display_draw_rectangle(WbDeviceTag tag, int x, int y, int width, int height) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (width <= 0) { + fprintf(stderr, "Error: %s(): 'width' argument is negative or null.\n", __FUNCTION__); + return; + } + if (height <= 0) { + fprintf(stderr, "Error: %s(): 'height' argument is negative or null.\n", __FUNCTION__); + return; + } + const int px[] = {x, width}; + const int py[] = {y, height}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_RECTANGLE, px, py, 2, false, NULL); +} + +void wb_display_draw_oval(WbDeviceTag tag, int cx, int cy, int a, int b) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (a <= 0) { + fprintf(stderr, "Error: %s(): 'horizontal_radius' argument is negative or null.\n", __FUNCTION__); + return; + } + if (b <= 0) { + fprintf(stderr, "Error: %s(): 'vertical_radius' argument is negative or null.\n", __FUNCTION__); + return; + } + const int px[] = {cx, a}; + const int py[] = {cy, b}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_OVAL, px, py, 2, false, NULL); +} + +void wb_display_draw_polygon(WbDeviceTag tag, const int *x, const int *y, int size) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (!x || !y) { + fprintf(stderr, "Error: %s(): 'x' or 'y' argument is NULL.\n", __FUNCTION__); + return; + } + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_POLYGON, x, y, size, false, NULL); +} + +void wb_display_draw_text(WbDeviceTag tag, const char *text, int x, int y) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (!text || strlen(text) == 0) { + fprintf(stderr, "Error: %s(): 'text' argument is NULL or empty.\n", __FUNCTION__); + return; + } + const int px[] = {x}; + const int py[] = {y}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_TEXT, px, py, 1, false, text); +} + +void wb_display_fill_rectangle(WbDeviceTag tag, int x, int y, int width, int height) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (width <= 0) { + fprintf(stderr, "Error: %s(): 'width' argument is negative or null.\n", __FUNCTION__); + return; + } + if (height <= 0) { + fprintf(stderr, "Error: %s(): 'height' argument is negative or null.\n", __FUNCTION__); + return; + } + const int px[] = {x, width}; + const int py[] = {y, height}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_RECTANGLE, px, py, 2, true, NULL); +} + +void wb_display_fill_oval(WbDeviceTag tag, int cx, int cy, int a, int b) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (a <= 0) { + fprintf(stderr, "Error: %s(): 'horizontal_radius' argument is negative or null.\n", __FUNCTION__); + return; + } + if (b <= 0) { + fprintf(stderr, "Error: %s(): 'vertical_radius' argument is negative or null.\n", __FUNCTION__); + return; + } + const int px[] = {cx, a}; + const int py[] = {cy, b}; + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_OVAL, px, py, 2, true, NULL); +} + +void wb_display_fill_polygon(WbDeviceTag tag, const int *x, const int *y, int size) { + const Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + if (!x || !y) { + fprintf(stderr, "Error: %s(): 'x' or 'y' arguments is NULL.\n", __FUNCTION__); + return; + } + wb_display_draw_primitive(tag, C_DISPLAY_DRAW_POLYGON, x, y, size, true, NULL); +} + +WbImageRef wb_display_image_copy(WbDeviceTag tag, int x, int y, int width, int height) { + if (width < 1 || height < 1) { + fprintf(stderr, "Error: %s(): 'width' or 'height' argument is invalid.\n", __FUNCTION__); + return NULL; + } + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + WbImageStruct *im = (WbImageStruct *)malloc(sizeof(WbImageStruct)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + free(m); + free(i); + free(im); + im = NULL; + } else if (m && i && im) { + m->message = C_DISPLAY_IMAGE_COPY; + i->id = d->image_next_free_id; + i->x = x; + i->y = y; + i->width = width; + i->height = height; + m->image_message = i; + message_enqueue(d, m); + im->id = d->image_next_free_id; + im->device_tag = tag; + d->image_next_free_id++; + } + robot_mutex_unlock(); + return im; +} + +void wb_display_image_paste(WbDeviceTag tag, WbImageRef ir, int x, int y, bool blend) { + if (!ir || ir->id <= 0) { + fprintf(stderr, "Error: %s(): invalid WbImageRef argument.\n", __FUNCTION__); + return; + } + if (ir->device_tag != tag) { + fprintf(stderr, "Error: %s(): invalid WbImageRef created by a different Display device.\n", __FUNCTION__); + return; + } + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + free(m); + free(i); + } else if (m && i) { + m->message = C_DISPLAY_IMAGE_PASTE; + i->id = ir->id; + i->x = x; + i->y = y; + i->blend = blend; + m->image_message = i; + message_enqueue(d, m); + } + robot_mutex_unlock(); +} + +WbImageRef wb_display_image_new(WbDeviceTag tag, int width, int height, const void *data, int format) { + if (!data) { + fprintf(stderr, "Error: %s(): 'data' argument is NULL.\n", __FUNCTION__); + return NULL; + } + if (width < 1 || height < 1) { + fprintf(stderr, "Error: %s(): 'width' or 'height' argument is invalid.\n", __FUNCTION__); + return NULL; + } + if (format != WB_IMAGE_RGB && format != WB_IMAGE_RGBA && format != WB_IMAGE_ARGB && format != WB_IMAGE_BGRA && + format != WB_IMAGE_ABGR) { + fprintf(stderr, "Error: %s(): 'format' argument is invalid.\n", __FUNCTION__); + return NULL; + } + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; + } + + robot_mutex_lock(); + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + WbImageStruct *im = (WbImageStruct *)malloc(sizeof(WbImageStruct)); + m->message = C_DISPLAY_IMAGE_LOAD; + m->image_message = i; + message_enqueue(d, m); + i->id = d->image_next_free_id; + i->width = width; + i->height = height; + i->format = format; + i->image = malloc(i->width * i->height * display_get_channel_number(i->format)); + + if (display_get_channel_number(i->format) == 3) + memcpy(i->image, data, i->width * i->height * display_get_channel_number(i->format)); + else { // channel == 4 + int j; + const int s = width * height; + const unsigned char *img = (unsigned char *)data; + for (j = 0; j < s; j++) + ((uint32_t *)i->image)[j] = img[j * 4 + 3] << 24 | img[j * 4 + 2] << 16 | img[j * 4 + 1] << 8 | img[j * 4]; + } + + im->id = d->image_next_free_id; + im->device_tag = tag; + d->image_next_free_id++; + robot_mutex_unlock(); + return im; +} + +WbImageRef wb_display_image_load(WbDeviceTag tag, const char *filename) { + if (!filename || strlen(filename) == 0) { + fprintf(stderr, "Error: %s(): 'filename' argument is NULL or empty.\n", __FUNCTION__); + return NULL; + } + GImage *gi = g_image_new(filename); + if (gi->failed || (gi->data_format != G_IMAGE_DATA_FORMAT_ABGR && gi->data_format != G_IMAGE_DATA_FORMAT_RGBA && + gi->data_format != G_IMAGE_DATA_FORMAT_RGB)) { + fprintf(stderr, "Error: %s(): the \"%s\" image is unreadable.\n", __FUNCTION__, filename); + g_image_delete(gi); + return NULL; + } + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + WbImageStruct *im = (WbImageStruct *)malloc(sizeof(WbImageStruct)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (!d) { + g_image_delete(gi); + free(m); + free(i); + free(im); + im = NULL; + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + } else if (m && i && im) { + m->message = C_DISPLAY_IMAGE_LOAD; + i->id = d->image_next_free_id; + switch (gi->data_format) { + case G_IMAGE_DATA_FORMAT_RGB: + i->format = WB_IMAGE_RGB; + break; + case G_IMAGE_DATA_FORMAT_RGBA: + i->format = WB_IMAGE_RGBA; + break; + case G_IMAGE_DATA_FORMAT_ABGR: + i->format = WB_IMAGE_ABGR; + break; + default: + assert(false); + } + i->width = gi->width; + i->height = gi->height; + i->image = gi->data; + m->image_message = i; + message_enqueue(d, m); + im->id = d->image_next_free_id; + im->device_tag = tag; + d->image_next_free_id++; + free(gi); // this should not delete the image data + } + robot_mutex_unlock(); + return im; +} + +void wb_display_image_save(WbDeviceTag tag, WbImageRef ir, const char *filename) { + if (!filename || strlen(filename) == 0) { + fprintf(stderr, "Error: %s(): 'filename' argument is NULL or empty.\n", __FUNCTION__); + return; + } + if (ir) { + if (ir->id <= 0) { + fprintf(stderr, "Error: %s(): invalid WbImageRef.\n", __FUNCTION__); + return; + } + if (ir->device_tag != tag) { + fprintf(stderr, "Error: %s(): invalid WbImageRef created by a different Display device.\n", __FUNCTION__); + return; + } + } + unsigned char type = g_image_get_type(filename); + if (type != G_IMAGE_PNG && type != G_IMAGE_JPEG) { + fprintf(stderr, "Error: %s(): unsupported file format. Supported file formats are \".jpg\" and \".png\".\n", __FUNCTION__); + return; + } + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + SaveOrder *o = (SaveOrder *)malloc(sizeof(SaveOrder)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + free(m); + free(o); + free(i); + } else if (m && i && o) { + const int id = ir ? ir->id : 0; + i->id = id; + o->id = id; + o->filename = (char *)malloc(strlen(filename) + 1); + o->type = type; + strcpy(o->filename, filename); + o->next = d->save_orders; + m->message = C_DISPLAY_IMAGE_SAVE; + m->image_message = i; + message_enqueue(d, m); + d->save_orders = o; + } + wb_robot_flush_unlocked(__FUNCTION__); + robot_mutex_unlock(); +} + +void wb_display_image_delete(WbDeviceTag tag, WbImageRef ir) { + if (!ir || ir->id <= 0) { + fprintf(stderr, "Error: %s(): invalid WbImageRef.\n", __FUNCTION__); + return; + } + if (ir->device_tag != tag) { + fprintf(stderr, "Error: %s(): invalid WbImageRef created by a different Display device.\n", __FUNCTION__); + return; + } + DisplayMessage *m = (DisplayMessage *)malloc(sizeof(DisplayMessage)); + DisplayImageMessage *i = (DisplayImageMessage *)malloc(sizeof(DisplayImageMessage)); + robot_mutex_lock(); + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + free(m); + free(i); + } else if (m && i) { + m->message = C_DISPLAY_IMAGE_DELETE; + i->id = ir->id; + m->image_message = i; + message_enqueue(d, m); + } + free(ir); + robot_mutex_unlock(); +} diff --git a/src/controller/c/display_private.h b/src/controller/c/display_private.h new file mode 100644 index 0000000..24698ea --- /dev/null +++ b/src/controller/c/display_private.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef DISPLAY_PRIVATE_H +#define DISPLAY_PRIVATE_H + +int display_get_channel_number(int pixel_format); + +#endif // DISPLAY_PRIVATE_H diff --git a/src/controller/c/distance_sensor.c b/src/controller/c/distance_sensor.c new file mode 100644 index 0000000..2223df6 --- /dev/null +++ b/src/controller/c/distance_sensor.c @@ -0,0 +1,247 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool enable; // need to enable device ? + int sampling_period; // milliseconds + double value; + WbDistanceSensorType type; + double max_value; + double min_value; + double aperture; + int lookup_table_size; + double *lookup_table; +} DistanceSensor; + +static DistanceSensor *distance_sensor_create() { + DistanceSensor *ds = malloc(sizeof(DistanceSensor)); + ds->enable = false; + ds->sampling_period = 0; + ds->value = NAN; + ds->type = WB_DISTANCE_SENSOR_GENERIC; + ds->max_value = 0; + ds->min_value = 0; + ds->aperture = 0; + ds->lookup_table = NULL; + ds->lookup_table_size = 0; + return ds; +} + +static DistanceSensor *distance_sensor_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_DISTANCE_SENSOR, true); + return d ? d->pdata : NULL; +} + +static void distance_sensor_read_answer(WbDevice *d, WbRequest *r) { + DistanceSensor *ds = (DistanceSensor *)d->pdata; + switch (request_read_uchar(r)) { + case C_DISTANCE_SENSOR_DATA: + ds->value = request_read_double(r); + break; + case C_CONFIGURE: + ds->type = request_read_int32(r); + ds->min_value = request_read_double(r); + ds->max_value = request_read_double(r); + ds->aperture = request_read_double(r); + ds->lookup_table_size = request_read_int32(r); + free(ds->lookup_table); + ds->lookup_table = NULL; + if (ds->lookup_table_size > 0) { + ds->lookup_table = (double *)malloc(sizeof(double) * ds->lookup_table_size * 3); + for (int i = 0; i < ds->lookup_table_size * 3; i++) + ds->lookup_table[i] = request_read_double(r); + } + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +int wb_distance_sensor_get_lookup_table_size(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->lookup_table_size; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_distance_sensor_get_lookup_table(WbDeviceTag tag) { + double *result = NULL; + robot_mutex_lock(); + DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->lookup_table; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void distance_sensor_write_request(WbDevice *d, WbRequest *r) { + DistanceSensor *ds = (DistanceSensor *)d->pdata; + if (ds->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, ds->sampling_period); + ds->enable = false; // done + } +} + +static void distance_sensor_cleanup(WbDevice *d) { + DistanceSensor *ds = (DistanceSensor *)d->pdata; + free(ds->lookup_table); + free(d->pdata); +} + +static void distance_sensor_toggle_remote(WbDevice *d, WbRequest *r) { + DistanceSensor *ds = (DistanceSensor *)d->pdata; + if (ds->sampling_period != 0) + ds->enable = true; +} + +void wbr_distance_sensor_set_value(WbDeviceTag t, double value) { + DistanceSensor *ds = distance_sensor_get_struct(t); + if (ds) + ds->value = value; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Protected functions (exported to WbDevice.cc) + +void wb_distance_sensor_init(WbDevice *d) { + d->pdata = distance_sensor_create(); + d->write_request = distance_sensor_write_request; + d->read_answer = distance_sensor_read_answer; + d->cleanup = distance_sensor_cleanup; + d->toggle_remote = distance_sensor_toggle_remote; +} + +// Public function available from the user API + +void wb_distance_sensor_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) { + ds->sampling_period = sampling_period; + ds->enable = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +int wb_distance_sensor_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + sampling_period = ds->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +void wb_distance_sensor_disable(WbDeviceTag tag) { + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + wb_distance_sensor_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +double wb_distance_sensor_get_value(WbDeviceTag tag) { + double value = NAN; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) { + if (ds->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_distance_sensor_enable().\n", __FUNCTION__); + value = ds->value; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return value; +} + +double wb_distance_sensor_get_max_value(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->max_value; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_distance_sensor_get_min_value(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->min_value; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_distance_sensor_get_aperture(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->aperture; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +WbDistanceSensorType wb_distance_sensor_get_type(WbDeviceTag tag) { + WbDistanceSensorType result = WB_DISTANCE_SENSOR_GENERIC; + robot_mutex_lock(); + const DistanceSensor *ds = distance_sensor_get_struct(tag); + if (ds) + result = ds->type; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/dynamic_library.c b/src/controller/c/dynamic_library.c new file mode 100644 index 0000000..8b9b642 --- /dev/null +++ b/src/controller/c/dynamic_library.c @@ -0,0 +1,86 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifdef _WIN32 +#include +#else // __linux__ || __APPLE__ +#include +#endif + +#include +#include +#include +#include "dynamic_library.h" + +#ifdef _WIN32 +#define DYNAMIC_LIBRARY_LOAD(a) LoadLibraryEx(wbu_system_short_path(a), NULL, LOAD_WITH_ALTERED_SEARCH_PATH) +#define DYNAMIC_LIBRARY_GETSYM(a, b) GetProcAddress(a, b) +#define DYNAMIC_LIBRARY_UNLOAD(a) FreeLibrary(a) + +#else // __linux__ || __APPLE__ +#define DYNAMIC_LIBRARY_LOAD(a) dlopen(a, RTLD_LAZY | RTLD_GLOBAL) +#define DYNAMIC_LIBRARY_GETSYM(a, b) dlsym(a, b) +#define DYNAMIC_LIBRARY_UNLOAD(a) dlclose(a) +#endif + +static void dynamic_library_print_last_error() { +#ifdef _WIN32 + LPVOID lpMsgBuf = NULL; + FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, NULL, + GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR)&lpMsgBuf, 0, NULL); + // cppcheck-suppress nullPointer + fprintf(stderr, "Error: %s (dynamic library)\n", (const char *)lpMsgBuf); + LocalFree(lpMsgBuf); +#else + const char *s = dlerror(); + if (!s) + fprintf(stderr, "Error: unknown error in dynamic library\n"); + else + fprintf(stderr, "Error: %s (dynamic library)\n", s); +#endif +} + +DYNAMIC_LIBRARY_HANDLE dynamic_library_init(const char *libname) { + const int length = strlen(libname); + if (length == 0) + return NULL; + char *tmpname = malloc(length + 4); + if (!tmpname) + return NULL; + memcpy(tmpname, libname, length); + tmpname[length] = '\0'; +#ifdef __linux__ + // dlopen() does not add .so to the filename, like windows does for .dll + if (length > 3 && (libname[length - 3] != '.' || libname[length - 2] != 's' || libname[length - 1] != 'o')) + strcat(tmpname, ".so"); +#endif + DYNAMIC_LIBRARY_HANDLE lib = (DYNAMIC_LIBRARY_HANDLE)DYNAMIC_LIBRARY_LOAD(tmpname); + if (!lib) + dynamic_library_print_last_error(); + free(tmpname); + return lib; +} + +void dynamic_library_cleanup(DYNAMIC_LIBRARY_HANDLE lib) { + if (lib) + DYNAMIC_LIBRARY_UNLOAD(lib); +} + +void *dynamic_library_get_symbol(DYNAMIC_LIBRARY_HANDLE lib, const char *symbol) { + if (!lib) + return NULL; + return (void *)DYNAMIC_LIBRARY_GETSYM(lib, symbol); +} diff --git a/src/controller/c/dynamic_library.h b/src/controller/c/dynamic_library.h new file mode 100644 index 0000000..37bb467 --- /dev/null +++ b/src/controller/c/dynamic_library.h @@ -0,0 +1,37 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef DYNAMIC_LIBRARY_H +#define DYNAMIC_LIBRARY_H + +#include +#include + +#ifdef _WIN32 +struct HINSTANCE__; +typedef struct HINSTANCE__ *dlInstance; +#define DYNAMIC_LIBRARY_HANDLE dlInstance +#else // __linux__ || __APPLE__ +#define DYNAMIC_LIBRARY_HANDLE void * +#endif + +DYNAMIC_LIBRARY_HANDLE dynamic_library_init(const char *libname); +bool dynamic_library_is_loaded(DYNAMIC_LIBRARY_HANDLE); +void dynamic_library_cleanup(DYNAMIC_LIBRARY_HANDLE); +void *dynamic_library_get_symbol(DYNAMIC_LIBRARY_HANDLE, const char *symbol); +const char *dynamic_library_get_last_error(); + +#endif // DYNAMIC_LIBRARY_H diff --git a/src/controller/c/emitter.c b/src/controller/c/emitter.c new file mode 100644 index 0000000..52085dc --- /dev/null +++ b/src/controller/c/emitter.c @@ -0,0 +1,340 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +//*************************************************************************** +// purpose: communication mechanism between controller and Webots +//*************************************************************************** + +#include +#include +#include +#include +#include +#include +#include "device_private.h" +#include "messages.h" +#include "robot_private.h" +#include "scheduler.h" + +#define UNKNOWN_CHANNEL -1 + +typedef struct _PacketStruct Packet; + +struct _PacketStruct { + int channel; // value of channel at the time that emitter_send...() was invoked + double range; // value of range at the time thet emitter_send...() was invoked + int size; // user data size (not including header) + char *data; // user data (packet body) + Packet *next; // next packet in emitter queue +}; + +static Packet *packet_create(const void *data, int size, int channel, double range) { + Packet *ps = malloc(sizeof(Packet)); + ps->channel = channel; + ps->range = range; + ps->size = size; + ps->data = malloc(size); + memcpy(ps->data, data, size); + ps->next = NULL; + return ps; +} + +static void packet_destroy(Packet *ps) { + if (ps->data) + free(ps->data); + free(ps); +} + +typedef struct { + int channel; // current emitter's channel + int buffer_used; // currently used buffer size + int buffer_size; // max buffer size (as in Emitter node) + double byte_rate; // max bytes sent per millisecond + double bytes_to_send; // bytes count according to byte_rate and elapsed time since the packet was enqueued + Packet *queue; // emission queue + double range; // current range + double max_range; // maximal range allowed + int *allowed_channels; // allowed channels emitter is allowed to emit to + int allowed_channels_size; // size of allowed_channels array + bool has_range_change; + bool has_channel_changed; +} Emitter; + +static Emitter *emitter_create() { + Emitter *es = malloc(sizeof(Emitter)); + es->channel = UNKNOWN_CHANNEL; + es->buffer_used = 0; + es->buffer_size = -1; + es->byte_rate = -1.0; + es->bytes_to_send = 0.0; + es->queue = NULL; + es->range = -1; + es->max_range = -1; + es->allowed_channels = NULL; + es->allowed_channels_size = -1; + es->has_range_change = false; + es->has_channel_changed = false; + return es; +} + +static void emitter_destroy(Emitter *es) { + Packet *ps = es->queue; + while (ps) { + Packet *garbage = ps; + ps = ps->next; + packet_destroy(garbage); + } + free(es->allowed_channels); + free(es); +} + +static void emitter_enqueue(Emitter *es, Packet *ps) { + if (es->queue) { + Packet *i = es->queue; + while (i->next) + i = i->next; + i->next = ps; + } else { + es->queue = ps; + es->bytes_to_send = 0.0; + } + + es->buffer_used += ps->size; +} + +static Packet *emitter_dequeue(Emitter *es) { + Packet *ps = es->queue; + es->queue = es->queue->next; + es->buffer_used -= ps->size; + return ps; +} + +// Static functions + +static inline WbDevice *emitter_get_device(WbDeviceTag t) { + return robot_get_device_with_node(t, WB_NODE_EMITTER, true); +} + +static void emitter_read_answer(WbDevice *d, WbRequest *r) { + switch (request_read_uchar(r)) { + case C_CONFIGURE: { + Emitter *es = d->pdata; + ROBOT_ASSERT(es->buffer_size == -1); + es->buffer_size = request_read_int32(r); + es->channel = request_read_int32(r); + es->byte_rate = request_read_double(r); + es->range = request_read_double(r); + es->max_range = request_read_double(r); + es->allowed_channels_size = request_read_int32(r); + es->allowed_channels = (int *)malloc(es->allowed_channels_size * sizeof(int)); + for (int i = 0; i < es->allowed_channels_size; i++) + es->allowed_channels[i] = request_read_int32(r); + break; + } + case C_EMITTER_SET_CHANNEL: { + Emitter *es = d->pdata; + es->channel = request_read_int32(r); + break; + } + case C_EMITTER_SET_RANGE: { + Emitter *es = d->pdata; + es->range = request_read_double(r); + break; + } + case C_EMITTER_SET_BUFFER_SIZE: { + Emitter *es = d->pdata; + es->buffer_size = request_read_int32(r); + break; + } + case C_EMITTER_SET_ALLOWED_CHANNELS: { + Emitter *es = d->pdata; + es->allowed_channels_size = request_read_int32(r); + es->allowed_channels = (int *)realloc(es->allowed_channels, es->allowed_channels_size * sizeof(int)); + for (int i = 0; i < es->allowed_channels_size; i++) + es->allowed_channels[i] = request_read_int32(r); + break; + } + default: + ROBOT_ASSERT(0); + } +} + +// send as many packets as the Emitter.baudRate allows +static void emitter_write_request(WbDevice *d, WbRequest *r) { + Emitter *es = d->pdata; + + if (es->queue && es->byte_rate != -1.0) + es->bytes_to_send += es->byte_rate * wb_robot_get_step_duration(); + + while (es->queue && (es->byte_rate == -1.0 || es->queue->size <= es->bytes_to_send)) { + Packet *ps = emitter_dequeue(es); + request_write_uchar(r, C_EMITTER_SEND); + request_write_int32(r, ps->channel); + request_write_double(r, ps->range); + request_write_int32(r, ps->size); + request_write_data(r, ps->data, ps->size); + es->bytes_to_send -= ps->size; + packet_destroy(ps); + } + + if (es->has_range_change) { + es->has_range_change = false; + request_write_uchar(r, C_EMITTER_SET_RANGE); + request_write_double(r, es->range); + } + + if (es->has_channel_changed) { + es->has_channel_changed = false; + request_write_uchar(r, C_EMITTER_SET_CHANNEL); + request_write_int32(r, es->channel); + } +} + +static void emitter_cleanup(WbDevice *d) { + emitter_destroy(d->pdata); +} + +// Exported functions + +void wb_emitter_init(WbDevice *d) { + d->read_answer = emitter_read_answer; + d->write_request = emitter_write_request; + d->cleanup = emitter_cleanup; + d->pdata = emitter_create(); +} + +// Public functions (available from the user API) + +int wb_emitter_send(WbDeviceTag tag, const void *data, int size) { + if (data == NULL) { + fprintf(stderr, "Error: %s(): invalid argument: data = NULL.\n", __FUNCTION__); + return 0; + } + + if (size < 1) { + fprintf(stderr, "Error: %s(): invalid size=%d argument.\n", __FUNCTION__, size); + return 0; + } + + int result = 0; + robot_mutex_lock(); + WbDevice *d = emitter_get_device(tag); + if (d) { + Emitter *es = d->pdata; + if (es->buffer_size == -1 || size <= es->buffer_size - es->buffer_used) { + emitter_enqueue(es, packet_create(data, size, es->channel, es->range)); + result = 1; + } + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +int wb_emitter_get_buffer_size(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const WbDevice *d = emitter_get_device(tag); + if (d) { + const Emitter *es = d->pdata; + result = es->buffer_size; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +int wb_emitter_get_channel(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const WbDevice *d = emitter_get_device(tag); + if (d) { + const Emitter *es = d->pdata; + result = es->channel; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_emitter_set_channel(WbDeviceTag tag, int channel) { + if (channel < -1) { + fprintf(stderr, "Error: %s() called with an invalid channel=%d. Please use a channel inside the range [-1,inf).\n", + __FUNCTION__, channel); + return; + } + + robot_mutex_lock(); + WbDevice *d = emitter_get_device(tag); + if (d) { + Emitter *es = d->pdata; + bool is_allowed = true; + + if (es->allowed_channels_size > 0) { + is_allowed = false; + for (int i = 0; i < es->allowed_channels_size; i++) { + if (es->allowed_channels[i] == channel) { + is_allowed = true; + break; + } + } + } + + if (!is_allowed) + fprintf(stderr, + "Error: %s() called with channel=%d, which is not between allowed channels. Please use an allowed channel.\n", + __FUNCTION__, channel); + else + es->channel = channel; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +double wb_emitter_get_range(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const WbDevice *d = emitter_get_device(tag); + if (d) { + const Emitter *es = d->pdata; + result = es->range; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_emitter_set_range(WbDeviceTag tag, double range) { + if (range < 0.0 && range != -1.0) { + fprintf(stderr, "Error: %s(): invalid range=%f argument.\n", __FUNCTION__, range); + return; + } + robot_mutex_lock(); + WbDevice *d = emitter_get_device(tag); + if (d) { + Emitter *es = d->pdata; + if (range == -1.0) // requested range is infinite + es->range = es->max_range; + else if (es->max_range == -1.0) // maxRange is infinite + es->range = range; + else if (range > es->max_range) + es->range = es->max_range; // clip requested value + else + es->range = range; // normal case + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} diff --git a/src/controller/c/file.c b/src/controller/c/file.c new file mode 100644 index 0000000..77dc941 --- /dev/null +++ b/src/controller/c/file.c @@ -0,0 +1,27 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "file.h" + +#include + +// src: http://stackoverflow.com/questions/5309471/getting-file-extension-in-c +const char *wb_file_get_extension(const char *filename) { + const char *dot = strrchr(filename, '.'); + if (!dot || dot == filename) + return ""; + return dot + 1; +} diff --git a/src/controller/c/file.h b/src/controller/c/file.h new file mode 100644 index 0000000..efdb7df --- /dev/null +++ b/src/controller/c/file.h @@ -0,0 +1,25 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef FILE_H +#define FILE_H + +#include +#include // for bool + +const char *wb_file_get_extension(const char *filename); // returns NULL in case of issues + +#endif diff --git a/src/controller/c/g_image.c b/src/controller/c/g_image.c new file mode 100644 index 0000000..793de04 --- /dev/null +++ b/src/controller/c/g_image.c @@ -0,0 +1,333 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "g_image.h" + +#include +#include +#include +#include + +#define STB_IMAGE_IMPLEMENTATION +#define STB_IMAGE_WRITE_IMPLEMENTATION +#define STBIW_WINDOWS_UTF8 +#include +#include + +#include + +// create a 64x64 pixel chess board image +static void g_image_make_chess_board(GImage *image) { + int i, j, idx; + unsigned char chessboard; + image->data = malloc(64 * 64 * 3); + image->data_format = G_IMAGE_DATA_FORMAT_RGB; + image->width = 64; + image->height = 64; + for (j = 0; j < 64; ++j) { + for (i = 0; i < 64; ++i) { + idx = i * 64 * 3 + j * 3; + chessboard = ((((i & 0x8) == 0) ? 1 : 0) ^ (((j & 0x8) == 0) ? 1 : 0)) * 255; + image->data[idx++] = chessboard; + image->data[idx++] = chessboard; + image->data[idx++] = chessboard; + } + } + image->flipped = false; + image->failed = true; +} + +static int g_image_file_not_found(const char *filename, GImage *image) { + fprintf(stderr, "Error: %s: file not found\n", filename); + g_image_make_chess_board(image); + return false; +} + +static int g_image_png_load(const char *filename, GImage *image) { + if (access(filename, F_OK) == -1) + return g_image_file_not_found(filename, image); + + int number_of_components; + image->data = stbi_load(filename, &image->width, &image->height, &number_of_components, 0); + + if (!image->data) + return false; + + if (number_of_components == STBI_rgb) + image->data_format = G_IMAGE_DATA_FORMAT_RGB; + else + image->data_format = G_IMAGE_DATA_FORMAT_RGBA; + return true; +} + +static int g_image_jpeg_load(const char *filename, GImage *image) { + if (access(filename, F_OK) == -1) + return g_image_file_not_found(filename, image); + + int number_of_components; + image->data = stbi_load(filename, &image->width, &image->height, &number_of_components, 0); + + if (!image->data) + return false; + + image->data_format = G_IMAGE_DATA_FORMAT_RGB; + + return true; +} + +unsigned char g_image_get_type(const char *filename) { + int l = strlen(filename); + if (((filename[l - 3] == 'j' || filename[l - 3] == 'J') && (filename[l - 2] == 'p' || filename[l - 2] == 'P') && + (filename[l - 1] == 'g' || filename[l - 1] == 'G')) || + ((filename[l - 4] == 'j' || filename[l - 4] == 'J') && (filename[l - 3] == 'p' || filename[l - 3] == 'P') && + (filename[l - 2] == 'e' || filename[l - 2] == 'E') && (filename[l - 1] == 'g' || filename[l - 1] == 'G'))) + return G_IMAGE_JPEG; + else if ((filename[l - 3] == 'p' || filename[l - 3] == 'P') && (filename[l - 2] == 'n' || filename[l - 2] == 'N') && + (filename[l - 1] == 'g' || filename[l - 1] == 'G')) + return G_IMAGE_PNG; + else if (((filename[l - 3] == 't' || filename[l - 3] == 'T') && (filename[l - 2] == 'i' || filename[l - 2] == 'I') && + (filename[l - 1] == 'f' || filename[l - 1] == 'F')) || + ((filename[l - 4] == 't' || filename[l - 4] == 'T') && (filename[l - 3] == 'i' || filename[l - 3] == 'I') && + (filename[l - 2] == 'f' || filename[l - 2] == 'F') && (filename[l - 1] == 'f' || filename[l - 1] == 'F'))) + return G_IMAGE_TIFF; + else if ((filename[l - 3] == 'h' || filename[l - 3] == 'H') && (filename[l - 2] == 'd' || filename[l - 2] == 'D') && + (filename[l - 1] == 'r' || filename[l - 1] == 'R')) + return G_IMAGE_HDR; + else + return G_IMAGE_NONE; +} + +void g_image_delete(GImage *image) { + free(image->data); + free(image); +} + +static int g_image_png_save(GImage *img, const char *filename) { + FILE *fd = fopen(filename, "wb"); + if (!fd) { + if (filename[0] == '/' +#ifdef _WIN32 + || (filename[1] == ':' && filename[2] == '\\') +#define DIR_SEPARATOR '\\' +#else +#define DIR_SEPARATOR '/' +#endif + ) + fprintf(stderr, "Insufficient permissions to write file: %s\n", filename); + else { + char cwd[256]; + const char *r = getcwd(cwd, 256); + if (r) + fprintf(stderr, "Insufficient permissions to write file: %s%c%s\n", cwd, DIR_SEPARATOR, filename); + else + fprintf(stderr, "Cannot get current directory for %s!\n", filename); + } + return -1; + } + fclose(fd); + + if (img->data_format == G_IMAGE_DATA_FORMAT_BGRA) { + unsigned char *image = (unsigned char *)malloc(4 * img->width * img->height); + int i; + for (i = 0; i < img->width * img->height; ++i) { + image[4 * i] = img->data[4 * i + 2]; + image[4 * i + 1] = img->data[4 * i + 1]; + image[4 * i + 2] = img->data[4 * i]; + image[4 * i + 3] = img->data[4 * i + 3]; + } + const int ret = stbi_write_png(filename, img->width, img->height, STBI_rgb_alpha, image, img->width * STBI_rgb_alpha); + free(image); + if (ret != 1) + return -1; + return 0; + } + + int number_of_components = STBI_rgb_alpha; + if (img->data_format == G_IMAGE_DATA_FORMAT_RGB) + number_of_components = STBI_rgb; + if (stbi_write_png(filename, img->width, img->height, number_of_components, img->data, img->width * number_of_components) != + 1) + return -1; + return 0; +} + +static int g_image_jpeg_save(GImage *img, char quality, const char *filename) { + FILE *fd = fopen(filename, "wb"); + if (!fd) { + fprintf(stderr, "Error: could not open \"%s\" for writing\n", filename); + return -1; + } + fclose(fd); + + if (img->data_format == G_IMAGE_DATA_FORMAT_BGRA) { + unsigned char *image = (unsigned char *)malloc(3 * img->width * img->height); + int i; + for (i = 0; i < img->width * img->height; ++i) { + image[3 * i] = img->data[4 * i + 2]; + image[3 * i + 1] = img->data[4 * i + 1]; + image[3 * i + 2] = img->data[4 * i]; + } + const int ret = stbi_write_jpg(filename, img->width, img->height, STBI_rgb, image, quality); + free(image); + if (ret != 1) + return -1; + return 0; + } + + if (stbi_write_jpg(filename, img->width, img->height, STBI_rgb, img->data, quality) != 1) + return -1; + return 0; +} + +static int g_image_hdr_save(GImage *img, const char *filename) { + FILE *fd = fopen(filename, "wb"); + if (!fd) { + fprintf(stderr, "Error: could not open \"%s\" for writing\n", filename); + return -1; + } + fclose(fd); + + if (stbi_write_hdr(filename, img->width, img->height, STBI_grey, img->float_data) != 1) + return -1; + return 0; +} + +int g_image_save(GImage *img, const char *filename, char quality) { + switch (g_image_get_type(filename)) { + case G_IMAGE_JPEG: + return g_image_jpeg_save(img, quality, filename); + case G_IMAGE_PNG: + return g_image_png_save(img, filename); + case G_IMAGE_HDR: + return g_image_hdr_save(img, filename); + default: + fprintf(stderr, "Cannot save: unsupported image type: %s\n", filename); + return -1; + } +} + +struct ImageData { + unsigned char **target_data; + unsigned long *target_data_size; +}; + +void g_image_save_to_jpeg_buffer_callback(void *context, void *data, int size) { + if (!*(((struct ImageData *)context)->target_data)) + *(((struct ImageData *)context)->target_data) = (unsigned char *)malloc(size); + else + *(((struct ImageData *)context)->target_data) = (unsigned char *)realloc( + *(((struct ImageData *)context)->target_data), *(((struct ImageData *)context)->target_data_size) + size); + memcpy(*(((struct ImageData *)context)->target_data) + *(((struct ImageData *)context)->target_data_size), data, size); + *(((struct ImageData *)context)->target_data_size) += size; +} + +int g_image_save_to_jpeg_buffer(GImage *img, unsigned char **target_data, unsigned long *target_data_size, char quality) { + struct ImageData imageData; + imageData.target_data = target_data; + imageData.target_data_size = target_data_size; + *target_data_size = 0; + + if (stbi_write_jpg_to_func((stbi_write_func *)&g_image_save_to_jpeg_buffer_callback, &imageData, img->width, img->height, + STBI_rgb, img->data, quality) != 1) + return -1; + + return 0; +} + +GImage *g_image_new(const char *filename) { + GImage *image = malloc(sizeof(GImage)); + image->failed = true; + switch (g_image_get_type(filename)) { + case G_IMAGE_JPEG: + image->failed = !g_image_jpeg_load(filename, image); + break; + case G_IMAGE_PNG: + image->failed = !g_image_png_load(filename, image); + break; + default: + g_image_make_chess_board(image); + fprintf(stderr, "Unsupported image type: %s\n", filename); + } + return image; +} + +void g_image_flip(GImage *im) { + int i, j; + const int width = im->width; + const int height = im->height; + unsigned char channel = im->data_format == G_IMAGE_DATA_FORMAT_RGB ? 3 : 4; + if (im->data == NULL) + return; + unsigned char *flipped_im = malloc(width * height * channel); + for (i = 0; i < height; ++i) { + for (j = 0; j < width; ++j) { + flipped_im[(i * width + width - 1 - j) * channel] = im->data[(i * width + j) * channel]; + flipped_im[(i * width + width - 1 - j) * channel + 1] = im->data[(i * width + j) * channel + 1]; + flipped_im[(i * width + width - 1 - j) * channel + 2] = im->data[(i * width + j) * channel + 2]; + if (channel == 4) + flipped_im[(i * width + width - 1 - j) * channel + 3] = im->data[(i * width + j) * channel + 3]; + } + } + free(im->data); + im->data = flipped_im; + im->flipped = !im->flipped; +} + +int g_image_downscale(GImage *img, int new_width, int new_height, float max_range) { + // assumptions: + // - img->data is cleared at the caller level. + // - (G_IMAGE_DATA_FORMAT_BGRA || G_IMAGE_DATA_FORMAT_F) -> G_IMAGE_DATA_FORMAT_RGB conversion. + // - new dimension is smaller than or equals to the old dimension. + assert(img->data_format == G_IMAGE_DATA_FORMAT_BGRA || img->data_format == G_IMAGE_DATA_FORMAT_F); + assert((new_width < img->width && new_height < img->height) || (new_width < img->width && new_height == img->height) || + (new_width == img->width && new_height < img->height) || + (new_width == img->width && new_height == img->height) // do only a BGRA -> RGB conversion. + ); + + unsigned char *new_data = malloc(new_width * new_height * 3); + if (!new_data) + return -1; + + const float width_ratio = (float)img->width / (float)new_width; + const float height_ratio = (float)img->height / (float)new_height; + const float _255_over_max = 255.0f / max_range; + int new_y, new_x, c; + for (new_y = 0; new_y < new_height; ++new_y) { + const int y = height_ratio * new_y + 0.5f; // +0.5 to round it. + const int line_index = y * img->width; + const int new_line_index = new_y * new_width; + for (new_x = 0; new_x < new_width; ++new_x) { + const int x = width_ratio * new_x + 0.5f; // +0.5 to round it. + const int new_column_index = 3 * (new_line_index + new_x); + if (img->data_format == G_IMAGE_DATA_FORMAT_BGRA) { + const int column_index = 4 * (line_index + x); + for (c = 0; c < 3; ++c) + new_data[new_column_index + c] = img->data[column_index + (2 - c)]; + } else { // img->data_format == G_IMAGE_DATA_FORMAT_F + const int column_index = line_index + x; + for (c = 0; c < 3; ++c) + new_data[new_column_index + c] = img->float_data[column_index] * _255_over_max; + } + } + } + + img->width = new_width; + img->height = new_height; + img->data = new_data; + img->float_data = NULL; + img->data_format = G_IMAGE_DATA_FORMAT_RGB; + return 0; +} diff --git a/src/controller/c/g_image.h b/src/controller/c/g_image.h new file mode 100644 index 0000000..9ae448b --- /dev/null +++ b/src/controller/c/g_image.h @@ -0,0 +1,62 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef G_IMAGE_H +#define G_IMAGE_H + +#define G_IMAGE_NONE 0 +#define G_IMAGE_PNG 1 +#define G_IMAGE_JPEG 2 +#define G_IMAGE_XPM 3 +#define G_IMAGE_TIFF 4 +#define G_IMAGE_HDR 5 + +#define G_IMAGE_DATA_FORMAT_F 0 +#define G_IMAGE_DATA_FORMAT_RGB 1 +#define G_IMAGE_DATA_FORMAT_ABGR 2 +#define G_IMAGE_DATA_FORMAT_BGRA 3 +#define G_IMAGE_DATA_FORMAT_RGBA 4 + +#ifdef __cplusplus +extern "C" { +#endif + +typedef struct _GImage GImage; + +struct _GImage { + int width; + int height; + float *float_data; + unsigned char *data; + unsigned char data_format; + char failed; + char flipped; +}; + +GImage *g_image_new(const char *filename); +void g_image_flip(GImage *); +void g_image_delete(GImage *); +int g_image_save(GImage *, const char *filename, char quality); +// the caller is responsible to free *target_data +int g_image_save_to_jpeg_buffer(GImage *img, unsigned char **target_data, unsigned long *target_data_size, char quality); +int g_image_downscale(GImage *img, int new_width, int new_height, float max_range); +unsigned char g_image_get_type(const char *filename); + +#ifdef __cplusplus +} +#endif + +#endif // G_IMAGE_H diff --git a/src/controller/c/g_pipe.c b/src/controller/c/g_pipe.c new file mode 100644 index 0000000..6c95e96 --- /dev/null +++ b/src/controller/c/g_pipe.c @@ -0,0 +1,148 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "g_pipe.h" + +#ifdef _WIN32 +#include +#include +#include +#else +#include +#include +#include +#endif // _WIN32 + +#include +#include +#include +#include +#include +#include "scheduler.h" + +GPipe *g_pipe_new(const char *path) { + GPipe *p = malloc(sizeof(GPipe)); +#ifdef _WIN32 + p->fd[0] = 0; + p->fd[1] = 0; + while (1) { + p->handle = CreateFile(path, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL); + if (p->handle != INVALID_HANDLE_VALUE) + break; + DWORD dwError = GetLastError(); + if (dwError != ERROR_PIPE_BUSY) { + free(p); + return NULL; + } + if (!WaitNamedPipe(path, 5000)) { + fprintf(stderr, "Cannot open pipe file: %s after trying for 5 seconds\n", path); + free(p); + return NULL; + } + } +#else + p->handle = socket(PF_UNIX, SOCK_STREAM, 0); + if (p->handle < 0) { + fprintf(stderr, "socket() failed\n"); + // cppcheck-suppress memleak ; otherwise cppcheck shows a false positive for p->handle + free(p); + return NULL; + } + struct sockaddr_un address; + memset(&address, 0, sizeof(struct sockaddr_un)); + address.sun_family = AF_UNIX; + strncpy(address.sun_path, path, sizeof(address.sun_path)); + if (connect(p->handle, (struct sockaddr *)&address, sizeof(struct sockaddr_un)) != 0) { + close(p->handle); + free(p); + return NULL; + } +#endif + return p; +} + +void g_pipe_delete(GPipe *p) { + if (p == NULL) + return; + if (p->handle) +#ifdef _WIN32 + CloseHandle(p->handle); +#else + close(p->handle); +#endif + free(p); +} + +static void broken_pipe() { + exit(1); +} + +void g_pipe_send(GPipe *p, const char *data, int size) { +#ifdef _WIN32 + assert(p->handle); + DWORD m = 0; + if (WriteFile(p->handle, data, size, &m, NULL) == 0) + broken_pipe(); +#else + int fd = p->handle; + if (!fd) + fd = p->fd[1]; + if (write(fd, data, size) == -1) + broken_pipe(); +#endif +} + +int g_pipe_receive(GPipe *p, char *data, int size) { +#ifdef _WIN32 + DWORD nb_read = 0; + DWORD e = ERROR_SUCCESS; + BOOL success = false; + assert(p->handle); + do { + success = ReadFile(p->handle, data, size, &nb_read, NULL); + if (!success) { + e = GetLastError(); + if (e != ERROR_MORE_DATA) + break; + e = ERROR_SUCCESS; + } + } while (!success); // repeat loop while ERROR_MORE_DATA + if (e != ERROR_SUCCESS) + broken_pipe(); + return (int)nb_read; +#else + int fd = p->handle; + if (!fd) + fd = p->fd[0]; + int n = read(fd, data, size); + // the read function returns -1 when a debugger + // is trying to attach to the process. + // (The controller is very frequently at this step, + // for example, when the simulator is paused) + // A second try is required in this case + if (n == -1 && errno == EINTR) + n = read(fd, data, size); + if (n <= 0) + broken_pipe(); + return n; +#endif +} + +size_t g_pipe_get_handle(const GPipe *p) { + if (p) + return (size_t)p->handle; + return 0; +} diff --git a/src/controller/c/g_pipe.h b/src/controller/c/g_pipe.h new file mode 100644 index 0000000..51b18d3 --- /dev/null +++ b/src/controller/c/g_pipe.h @@ -0,0 +1,55 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef G_PIPE_H +#define G_PIPE_H + +#ifdef _WIN32 +#include +#endif // _WIN32 +#include + +#ifdef __cplusplus +extern "C" { +#endif + +struct _GPipe { + int fd[2]; +#ifdef _WIN32 + HANDLE handle; + char *buffer; + int pointer; + int read; + int buffer_size; + int fd_local[2]; // for the local pipe +#else + int handle; +#endif +}; + +typedef struct _GPipe GPipe; + +GPipe *g_pipe_new(const char *); // named pipe on Windows, UNIX domain socket on Linux / macOS +void g_pipe_delete(GPipe *); +void g_pipe_send(GPipe *, const char *data, int size); +int g_pipe_receive(GPipe *, char *data, int size); +size_t g_pipe_get_handle(const GPipe *); + +#ifdef __cplusplus +} +#endif + +#endif // G_PIPE_H diff --git a/src/controller/c/gps.c b/src/controller/c/gps.c new file mode 100644 index 0000000..1e20040 --- /dev/null +++ b/src/controller/c/gps.c @@ -0,0 +1,231 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool enable; + int sampling_period; + WbGpsCoordinateSystem coordinate_system; + double position[3]; + double speed; + double motion[3]; +} GPS; + +static GPS *gps_create() { + GPS *gps = malloc(sizeof(GPS)); + gps->enable = false; + gps->sampling_period = 0; + gps->coordinate_system = WB_GPS_LOCAL_COORDINATE; + gps->position[0] = NAN; + gps->position[1] = NAN; + gps->position[2] = NAN; + gps->speed = NAN; + gps->motion[0] = NAN; + gps->motion[1] = NAN; + gps->motion[2] = NAN; + return gps; +} + +static GPS *gps_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_GPS, true); + return d ? d->pdata : NULL; +} + +static void gps_read_answer(WbDevice *d, WbRequest *r) { + GPS *gps = d->pdata; + int i; + + switch (request_read_uchar(r)) { + case C_GPS_DATA: + for (i = 0; i < 3; i++) + gps->position[i] = request_read_double(r); + gps->speed = request_read_double(r); + for (i = 0; i < 3; i++) + gps->motion[i] = request_read_double(r); + break; + case C_CONFIGURE: + gps->coordinate_system = request_read_int32(r); + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +static void gps_write_request(WbDevice *d, WbRequest *r) { + GPS *gps = d->pdata; + if (gps->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, gps->sampling_period); + gps->enable = false; + } +} + +static void gps_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void gps_toggle_remote(WbDevice *d, WbRequest *r) { + GPS *gps = d->pdata; + if (gps->sampling_period != 0) + gps->enable = true; +} + +void wbr_gps_set_values(WbDeviceTag t, const double *values) { + GPS *gps = gps_get_struct(t); + if (gps) { + gps->position[0] = values[0]; + gps->position[1] = values[1]; + gps->position[2] = values[2]; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wbr_gps_set_velocity_vector(WbDeviceTag t, const double *values) { + GPS *gps = gps_get_struct(t); + if (gps) { + gps->motion[0] = values[0]; + gps->motion[1] = values[1]; + gps->motion[2] = values[2]; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wbr_gps_set_speed(WbDeviceTag t, const double speed) { + GPS *gps = gps_get_struct(t); + if (gps) + gps->speed = speed; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Protected functions (exported to device.cc) + +void wb_gps_init(WbDevice *d) { + d->write_request = gps_write_request; + d->read_answer = gps_read_answer; + d->cleanup = gps_cleanup; + d->pdata = gps_create(); + d->toggle_remote = gps_toggle_remote; +} + +// Public function available from the user API + +void wb_gps_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + GPS *gps = gps_get_struct(tag); + if (gps) { + gps->enable = true; + gps->sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_gps_disable(WbDeviceTag tag) { + const GPS *gps = gps_get_struct(tag); + if (gps) + wb_gps_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_gps_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const GPS *gps = gps_get_struct(tag); + if (gps) + sampling_period = gps->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +const double *wb_gps_get_values(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const GPS *gps = gps_get_struct(tag); + if (gps) { + if (gps->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_gps_enable().\n", __FUNCTION__); + result = gps->position; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_gps_get_speed(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const GPS *gps = gps_get_struct(tag); + if (gps) { + if (gps->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_gps_enable().\n", __FUNCTION__); + result = gps->speed; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_gps_get_speed_vector(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const GPS *gps = gps_get_struct(tag); + if (gps) { + if (gps->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_gps_enable().\n", __FUNCTION__); + result = gps->motion; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const char *wb_gps_convert_to_degrees_minutes_seconds(double decimal_degrees) { + int degrees = decimal_degrees; + int minutes = (decimal_degrees - degrees) * 60; + int seconds = (((decimal_degrees - degrees) * 60) - minutes) * 60; + char *buffer = (char *)malloc(32 * sizeof(char)); + sprintf(buffer, "%d° %d′ %d″", degrees, minutes, seconds); + return buffer; +} + +WbGpsCoordinateSystem wb_gps_get_coordinate_system(WbDeviceTag tag) { + WbGpsCoordinateSystem result = WB_GPS_LOCAL_COORDINATE; + robot_mutex_lock(); + const GPS *gps = gps_get_struct(tag); + if (gps) + result = gps->coordinate_system; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/gyro.c b/src/controller/c/gyro.c new file mode 100644 index 0000000..70c911b --- /dev/null +++ b/src/controller/c/gyro.c @@ -0,0 +1,191 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +typedef struct { + int enable; // need to enable device ? + int sampling_period; // milliseconds + double velocity[3]; // angular velocity + int lookup_table_size; + double *lookup_table; +} Gyro; + +static Gyro *gyro_create() { + Gyro *gyro = malloc(sizeof(Gyro)); + gyro->enable = false; + gyro->sampling_period = 0; + gyro->velocity[0] = NAN; + gyro->velocity[1] = NAN; + gyro->velocity[2] = NAN; + gyro->lookup_table = NULL; + gyro->lookup_table_size = 0; + return gyro; +} + +// Static functions + +static Gyro *gyro_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_GYRO, true); + return d ? d->pdata : NULL; +} + +static void gyro_read_answer(WbDevice *d, WbRequest *r) { + Gyro *gyro = d->pdata; + switch (request_read_uchar(r)) { + case C_GYRO_DATA: + gyro->velocity[0] = request_read_double(r); + gyro->velocity[1] = request_read_double(r); + gyro->velocity[2] = request_read_double(r); + break; + case C_CONFIGURE: + gyro->lookup_table_size = request_read_int32(r); + free(gyro->lookup_table); + gyro->lookup_table = NULL; + if (gyro->lookup_table_size > 0) { + gyro->lookup_table = (double *)malloc(sizeof(double) * gyro->lookup_table_size * 3); + for (int i = 0; i < gyro->lookup_table_size * 3; i++) + gyro->lookup_table[i] = request_read_double(r); + } + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +int wb_gyro_get_lookup_table_size(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const Gyro *dev = gyro_get_struct(tag); + if (dev) + result = dev->lookup_table_size; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_gyro_get_lookup_table(WbDeviceTag tag) { + double *result = NULL; + robot_mutex_lock(); + Gyro *dev = gyro_get_struct(tag); + if (dev) + result = dev->lookup_table; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void gyro_write_request(WbDevice *d, WbRequest *r) { + Gyro *gyro = d->pdata; + if (gyro->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, gyro->sampling_period); + gyro->enable = false; // done + } +} + +static void gyro_cleanup(WbDevice *d) { + Gyro *gyro = (Gyro *)d->pdata; + free(gyro->lookup_table); + free(d->pdata); +} + +static void gyro_toggle_remote(WbDevice *d, WbRequest *r) { + Gyro *gyro = d->pdata; + if (gyro->sampling_period != 0) + gyro->enable = true; +} + +void wbr_gyro_set_values(WbDeviceTag t, const double *values) { + Gyro *gyro = gyro_get_struct(t); + if (gyro) { + gyro->velocity[0] = values[0]; + gyro->velocity[1] = values[1]; + gyro->velocity[2] = values[2]; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wb_gyro_init(WbDevice *); + +void wb_gyro_init(WbDevice *d) { + d->write_request = gyro_write_request; + d->read_answer = gyro_read_answer; + d->cleanup = gyro_cleanup; + d->pdata = gyro_create(); + d->toggle_remote = gyro_toggle_remote; +} + +// Public function available from the user API + +void wb_gyro_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Gyro *gyro = gyro_get_struct(tag); + if (gyro) { + gyro->enable = true; + gyro->sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_gyro_disable(WbDeviceTag tag) { + const Gyro *gyro = gyro_get_struct(tag); + if (gyro) + wb_gyro_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_gyro_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Gyro *gyro = gyro_get_struct(tag); + if (gyro) + sampling_period = gyro->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +const double *wb_gyro_get_values(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const Gyro *gyro = gyro_get_struct(tag); + if (gyro) { + if (gyro->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_gyro_enable().\n", __FUNCTION__); + result = gyro->velocity; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/html_robot_window.c b/src/controller/c/html_robot_window.c new file mode 100644 index 0000000..ec9b720 --- /dev/null +++ b/src/controller/c/html_robot_window.c @@ -0,0 +1,112 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +// This module managemes the shared library associated with an HTML robot +// window. +// Note: this file should be renamed to robot_window.c when the Qt robot +// windows are disabled. + +#include +#include "dynamic_library.h" +#include "html_robot_window_private.h" + +#include +#include +#include +#include + +static void (*wb_robot_window_init)(void) = NULL; +static void (*wb_robot_window_step)(int) = NULL; +static void (*wb_robot_window_cleanup)(void) = NULL; + +static DYNAMIC_LIBRARY_HANDLE library_handle = NULL; + +static void wb_robot_window_unload_library() { + wb_robot_window_step = NULL; + dynamic_library_cleanup(library_handle); + library_handle = NULL; +} + +bool wb_robot_window_load_library(const char *name) { + if (name[0] == '\0') + return false; + // search for the corresponding HTML file, e.g., /fullpath/e-puck.dll => /fullpath/e-puck.html + // On windows, the name is /fullpath/e-puck.dll + // On Linux, the name is /fullpath/libe-puck.so + // on macOS, the name is /fullpath/libe-puck.dylib + int l = strlen(name); + assert(l > 3); + char *html = (char *)malloc(l + 6); + memcpy(html, name, l + 1); +#ifndef _WIN32 // we have to remove the lib prefix + int slash; + for (slash = l; html[slash] != '/' && slash >= 0; slash--) { + } + if (slash < 0) { + free(html); + return true; + } + l -= 3; + int i; + for (i = slash + 1; i < l; i++) + html[i] = html[i + 3]; + html[i] = '\0'; +#endif + int dot; + for (dot = l; dot > 0 && html[dot] != '.'; dot--) { + } + html[dot + 1] = 'h'; + html[dot + 2] = 't'; + html[dot + 3] = 'm'; + html[dot + 4] = 'l'; + html[dot + 5] = '\0'; + // if the corresponding HTML file doesn't exists, it is an old Qt robot window + // and we don't want to open it here + if (access(wbu_system_short_path(html), F_OK) == -1) { + free(html); + return true; + } + free(html); + library_handle = dynamic_library_init(name); + if (!library_handle) { + fprintf(stderr, "Error: failed to load %s library\n", name); + return false; + } + wb_robot_window_init = (void (*)())dynamic_library_get_symbol(library_handle, "wb_robot_window_init"); + wb_robot_window_step = (void (*)(int))dynamic_library_get_symbol(library_handle, "wb_robot_window_step"); + wb_robot_window_cleanup = (void (*)())dynamic_library_get_symbol(library_handle, "wb_robot_window_cleanup"); + if (!wb_robot_window_step) { + wb_robot_window_unload_library(); + fprintf(stderr, "Error: cannot find any 'void wb_robot_window_step(int)' function in the %s robot window library\n", name); + return false; + } + return true; +} + +void html_robot_window_init() { + if (wb_robot_window_init) + (*wb_robot_window_init)(); +} + +void html_robot_window_step(int step) { + if (wb_robot_window_step) + (*wb_robot_window_step)(step); +} + +void html_robot_window_cleanup() { + if (wb_robot_window_cleanup) + (*wb_robot_window_cleanup)(); +} diff --git a/src/controller/c/html_robot_window_private.h b/src/controller/c/html_robot_window_private.h new file mode 100644 index 0000000..55a60af --- /dev/null +++ b/src/controller/c/html_robot_window_private.h @@ -0,0 +1,25 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef HTML_ROBOT_WINDOW_PRIVATE_H +#define HTML_ROBOT_WINDOW_PRIVATE_H + +bool wb_robot_window_load_library(const char *name); +void html_robot_window_init(); +void html_robot_window_step(int step); +void html_robot_window_cleanup(); + +#endif // HTML_ROBOT_WINDOW__PRIVATE_H diff --git a/src/controller/c/image.c b/src/controller/c/image.c new file mode 100644 index 0000000..7e9b65f --- /dev/null +++ b/src/controller/c/image.c @@ -0,0 +1,74 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "image_private.h" + +#include +#include "robot_private.h" + +#ifdef _WIN32 +#include +#else // memory mapped files +#include +#include +#include +#include +#endif +#include + +Image *image_new() { + Image *i = malloc(sizeof(Image)); + i->filename = NULL; + i->size = 0; + i->data = NULL; + return i; +} + +void image_cleanup(Image *i) { + if (i->size <= 0) + return; + +#ifdef _WIN32 + if (i->fd != NULL) { + UnmapViewOfFile(i->data); + CloseHandle(i->fd); + } +#else + if (i->data > 0) + munmap(i->data, i->size); +#endif +} + +void image_setup(Image *i, WbRequest *r) { + i->size = request_read_int32(r); + if (i->size > 0) { + i->filename = request_read_string(r); + image_get(i); + } +} + +void image_get(Image *i) { +#ifdef _WIN32 + i->fd = OpenFileMapping(FILE_MAP_WRITE, FALSE, i->filename); + ROBOT_ASSERT(i->fd); + i->data = MapViewOfFile(i->fd, FILE_MAP_WRITE, 0, 0, 0); +#else // memory mapped files + int fd = open(i->filename, O_RDWR, 0400); + i->data = (unsigned char *)mmap(0, i->size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0); + close(fd); +#endif + ROBOT_ASSERT(i->data); +} diff --git a/src/controller/c/image_private.h b/src/controller/c/image_private.h new file mode 100644 index 0000000..9247b70 --- /dev/null +++ b/src/controller/c/image_private.h @@ -0,0 +1,39 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef IMAGE_PRIVATE_H +#define IMAGE_PRIVATE_H + +#ifdef _WIN32 +#include +#endif +#include "request.h" + +typedef struct { +#ifdef _WIN32 + HANDLE fd; +#endif + char *filename; + int size; + unsigned char *data; +} Image; + +Image *image_new(); +void image_cleanup(Image *i); +void image_setup(Image *i, WbRequest *r); +void image_get(Image *i); + +#endif // IMAGE_PRIVATE_H diff --git a/src/controller/c/inertial_unit.c b/src/controller/c/inertial_unit.c new file mode 100644 index 0000000..64a97e7 --- /dev/null +++ b/src/controller/c/inertial_unit.c @@ -0,0 +1,195 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +typedef struct { + int enable; // need to enable device ? + int sampling_period; // milliseconds + double quaternion[4]; + double noise; + char *coordinate_system; +} InertialUnit; + +static InertialUnit *inertial_unit_create() { + InertialUnit *inertial_unit = malloc(sizeof(InertialUnit)); + inertial_unit->enable = false; + inertial_unit->sampling_period = 0; + inertial_unit->coordinate_system = NULL; + return inertial_unit; +} + +// Static functions + +static InertialUnit *inertial_unit_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_INERTIAL_UNIT, true); + return d ? d->pdata : NULL; +} + +static void inertial_unit_read_answer(WbDevice *d, WbRequest *r) { + InertialUnit *s = d->pdata; + + switch (request_read_uchar(r)) { + case C_INERTIAL_UNIT_DATA: + for (int i = 0; i < 4; i++) + s->quaternion[i] = request_read_double(r); + break; + case C_CONFIGURE: + s->noise = request_read_double(r); + s->coordinate_system = request_read_string(r); + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +double wb_inertial_unit_get_noise(WbDeviceTag tag) { + double result = 0; + robot_mutex_lock(); + const InertialUnit *dev = inertial_unit_get_struct(tag); + if (dev) + result = dev->noise; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void inertial_unit_write_request(WbDevice *d, WbRequest *r) { + InertialUnit *inertial_unit = d->pdata; + if (inertial_unit->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, inertial_unit->sampling_period); + inertial_unit->enable = false; // done + } +} + +static void inertial_unit_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void inertial_unit_toggle_remote(WbDevice *d, WbRequest *r) { + InertialUnit *inertial_unit = d->pdata; + if (inertial_unit->sampling_period != 0) + inertial_unit->enable = true; +} + +void wb_inertial_unit_init(WbDevice *); + +void wb_inertial_unit_init(WbDevice *d) { + d->write_request = inertial_unit_write_request; + d->read_answer = inertial_unit_read_answer; + d->cleanup = inertial_unit_cleanup; + d->pdata = inertial_unit_create(); + d->toggle_remote = inertial_unit_toggle_remote; +} + +// Public function available from the user API + +void wb_inertial_unit_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + InertialUnit *inertial_unit = inertial_unit_get_struct(tag); + if (inertial_unit) { + inertial_unit->enable = true; + inertial_unit->sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_inertial_unit_disable(WbDeviceTag tag) { + const InertialUnit *inertial_unit = inertial_unit_get_struct(tag); + if (inertial_unit) + wb_inertial_unit_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_inertial_unit_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const InertialUnit *inertial_unit = inertial_unit_get_struct(tag); + if (inertial_unit) + sampling_period = inertial_unit->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +const double *wb_inertial_unit_get_roll_pitch_yaw(WbDeviceTag tag) { + static double result[3]; + robot_mutex_lock(); + const InertialUnit *inertial_unit = inertial_unit_get_struct(tag); + if (inertial_unit) { + if (inertial_unit->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_inertial_unit_enable().\n", __FUNCTION__); + + const double *q = inertial_unit->quaternion; + + if (strcmp(inertial_unit->coordinate_system, "NUE") == 0) { + // NUE: extrensic rotation matrix e = Y(yaw) Z(pitch) X(roll) + result[2] = atan2(2 * q[1] * q[3] - 2 * q[0] * q[2], 1 - 2 * q[1] * q[1] - 2 * q[2] * q[2]); + result[0] = atan2(2 * q[0] * q[3] - 2 * q[1] * q[2], 1 - 2 * q[0] * q[0] - 2 * q[2] * q[2]); + result[1] = asin(2 * q[0] * q[1] + 2 * q[2] * q[3]); + } else { + // ENU: extrensic rotation matrix e = Z(yaw) Y(pitch) X(roll) + const double t0 = 2.0 * (q[3] * q[0] + q[1] * q[2]); + const double t1 = 1.0 - 2.0 * (q[0] * q[0] + q[1] * q[1]); + const double roll = atan2(t0, t1); + double t2 = 2.0 * (q[3] * q[1] - q[2] * q[0]); + t2 = (t2 > 1.0) ? 1.0 : t2; + t2 = (t2 < -1.0) ? -1.0 : t2; + const double pitch = asin(t2); + const double t3 = 2.0 * (q[3] * q[2] + q[0] * q[1]); + const double t4 = 1.0 - 2.0 * (q[1] * q[1] + q[2] * q[2]); + const double yaw = atan2(t3, t4); + result[0] = roll; + result[1] = pitch; + result[2] = yaw; + } + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_inertial_unit_get_quaternion(WbDeviceTag tag) { + const double *result = NULL; + robot_mutex_lock(); + const InertialUnit *inertial_unit = inertial_unit_get_struct(tag); + if (inertial_unit) { + if (inertial_unit->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_inertial_unit_enable().\n", __FUNCTION__); + result = inertial_unit->quaternion; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/joystick.c b/src/controller/c/joystick.c new file mode 100644 index 0000000..07cd38a --- /dev/null +++ b/src/controller/c/joystick.c @@ -0,0 +1,352 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "joystick_private.h" +#include "messages.h" +#include "robot_private.h" + +#include +#include + +typedef struct { + int sampling_period; + int connected; + int button_pointer; + int number_of_axes; + int number_of_povs; + int number_of_buttons; + int *pressed_button; + int *axis_value; + int *pov_value; + char *model; + // force feedback + int force_feedback_level; + double force_feedback_duration; + double auto_centering_gain; + double resistance_gain; + int force_axis; + bool force_feedback_level_request; + bool force_feedback_duration_request; + bool auto_centering_gain_request; + bool resistance_gain_request; + bool force_axis_request; +} WbJoystick; + +static WbJoystick joystick; + +static void joystick_read_value(WbRequest *r) { + int i; + // read buttons + int number_of_pressed_button = request_read_uint32(r); + for (i = 0; i < number_of_pressed_button; ++i) { + if (i >= joystick.number_of_buttons || joystick.pressed_button == NULL) + request_read_uint32(r); /* empty request */ + else + joystick.pressed_button[i] = request_read_uint32(r); + // fprintf(stdout, "Pressed button %d / %d.\n", joystick.pressed_button[i], number_of_pressed_button); + } + if (number_of_pressed_button > joystick.number_of_buttons) + number_of_pressed_button = joystick.number_of_buttons; + if (joystick.pressed_button) { + joystick.pressed_button[number_of_pressed_button] = -1; + if (joystick.button_pointer != -1) + joystick.button_pointer = 0; + } + // read axes + joystick.number_of_axes = request_read_uint32(r); + for (i = 0; i < joystick.number_of_axes; ++i) { + int value = request_read_uint32(r); + // fprintf(stdout, "Received %d for axis %d.\n", value, i); + if (i < joystick.number_of_axes && joystick.axis_value) + joystick.axis_value[i] = value; + } + // read povs + joystick.number_of_povs = request_read_uint32(r); + for (i = 0; i < joystick.number_of_povs; ++i) { + int value = request_read_uint32(r); + // fprintf(stdout, "Received %d for pov %d.\n", value, i); + if (i < joystick.number_of_povs && joystick.pov_value) + joystick.pov_value[i] = value; + } +} + +// Protected funtions available from other files of the client library + +void joystick_write_request(WbRequest *req) { + if (joystick.button_pointer == -1) { // need to enable or disable + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_SAMPLING_PERIOD); + request_write_uint16(req, joystick.sampling_period); + joystick.button_pointer = 0; + } + + if (joystick.force_feedback_level_request) { + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_FORCE_FEEDBACK); + request_write_uint16(req, joystick.force_feedback_level); + joystick.force_feedback_level_request = false; + } + + if (joystick.force_feedback_duration_request) { + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_FORCE_FEEDBACK_DURATION); + request_write_double(req, joystick.force_feedback_duration); + joystick.force_feedback_duration_request = false; + } + + if (joystick.auto_centering_gain_request) { + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_AUTO_CENTERING_GAIN); + request_write_double(req, joystick.auto_centering_gain); + joystick.auto_centering_gain_request = false; + } + + if (joystick.resistance_gain_request) { + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_RESISTANCE_GAIN); + request_write_double(req, joystick.resistance_gain); + joystick.resistance_gain_request = false; + } + + if (joystick.force_axis_request) { + request_write_uchar(req, C_ROBOT_SET_JOYSTICK_FORCE_AXIS); + request_write_uint32(req, joystick.force_axis); + joystick.force_axis_request = false; + } +} + +void joystick_set_sampling_period(int sampling_period) { + joystick.sampling_period = sampling_period; + joystick.button_pointer = -1; +} + +bool joystick_read_answer(int message, WbRequest *r) { + if (message == C_ROBOT_JOYSTICK_VALUE) { + joystick_read_value(r); + return true; + } else if (message == C_ROBOT_JOYSTICK_CONFIG) { + free(joystick.pressed_button); + joystick.pressed_button = NULL; + free(joystick.axis_value); + joystick.axis_value = NULL; + free(joystick.pov_value); + joystick.pov_value = NULL; + free(joystick.model); + joystick.model = NULL; + joystick.number_of_axes = request_read_uint32(r); + joystick.number_of_buttons = request_read_uint32(r); + joystick.number_of_povs = request_read_uint32(r); + if (joystick.number_of_axes < 0 && joystick.number_of_buttons < 0 && joystick.number_of_povs < 0) { + joystick.number_of_axes = 0; + joystick.number_of_buttons = 0; + joystick.number_of_povs = 0; + joystick.connected = false; + } else { + joystick.model = request_read_string(r); + joystick.axis_value = (int *)malloc(joystick.number_of_axes * sizeof(int)); + joystick.pov_value = (int *)malloc(joystick.number_of_povs * sizeof(int)); + joystick.pressed_button = (int *)malloc((joystick.number_of_buttons + 1) * sizeof(int)); + joystick.connected = true; + int i; + for (i = 0; i < joystick.number_of_axes; ++i) + joystick.axis_value[i] = 0; + for (i = 0; i < joystick.number_of_povs; ++i) + joystick.pov_value[i] = 0; + for (i = 0; i < (joystick.number_of_buttons + 1); ++i) + joystick.pressed_button[i] = -1; + } + return true; + } else + return false; +} + +void joystick_step_end() { + if (joystick.sampling_period) { + if (joystick.pressed_button) + joystick.pressed_button[0] = -1; + if (joystick.button_pointer != -1) + joystick.button_pointer = 0; + } +} + +void wb_joystick_init() { + joystick.sampling_period = 0; // initially disabled + joystick.connected = false; + joystick.button_pointer = -1; + joystick.number_of_axes = 0; + joystick.number_of_povs = 0; + joystick.number_of_buttons = 0; + joystick.pressed_button = NULL; + joystick.axis_value = NULL; + joystick.pov_value = NULL; + joystick.model = NULL; + joystick.force_feedback_level = 0; + joystick.force_feedback_duration = 0; + joystick.auto_centering_gain = 0.0; + joystick.resistance_gain = 0.0; + joystick.force_axis = 0; + joystick.force_feedback_level_request = false; + joystick.force_feedback_duration_request = false; + joystick.auto_centering_gain_request = false; + joystick.resistance_gain_request = false; + joystick.force_axis_request = false; +} + +// Public functions available from the keyboard API + +void wb_joystick_enable(int sampling_period) { + robot_mutex_lock(); + joystick.button_pointer = -1; // need to enable or disable + joystick.sampling_period = sampling_period; + robot_mutex_unlock(); +} + +void wb_joystick_disable() { + wb_joystick_enable(0); +} + +int wb_joystick_get_sampling_period() { + int sampling_period = 0; + robot_mutex_lock(); + sampling_period = joystick.sampling_period; + robot_mutex_unlock(); + return sampling_period; +} + +int wb_joystick_get_number_of_axes() { + if (joystick.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + + return joystick.number_of_axes; +} + +int wb_joystick_get_axis_value(int axis) { + if (joystick.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + + if (axis >= joystick.number_of_axes) + fprintf(stderr, "Error: %s() called with an 'axis' argument (%d) bigger than or equal to the number of axes (%d).\n", + __FUNCTION__, axis, joystick.number_of_axes); + + if (joystick.axis_value) + return joystick.axis_value[axis]; + else + return 0; +} + +int wb_joystick_get_number_of_povs() { + if (joystick.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + + return joystick.number_of_povs; +} + +int wb_joystick_get_pov_value(int pov) { + if (joystick.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + + if (pov >= joystick.number_of_povs) + fprintf(stderr, "Error: %s() called with a 'pov' argument (%d) bigger than or equal to the number of axes (%d).\n", + __FUNCTION__, pov, joystick.number_of_povs); + + if (joystick.pov_value) + return joystick.pov_value[pov]; + else + return 0; +} + +int wb_joystick_get_pressed_button() { + if (joystick.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + + if (joystick.button_pointer == -1 || joystick.pressed_button == NULL) + return -1; + int button = joystick.pressed_button[(int)joystick.button_pointer]; + if (button >= 0) + joystick.button_pointer++; + return button; +} + +void wb_joystick_set_constant_force(int level) { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return; + } + + joystick.force_feedback_level = level; + joystick.force_feedback_level_request = true; +} + +void wb_joystick_set_constant_force_duration(double duration) { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return; + } + + if (duration < 0) { + fprintf(stderr, "Error: %s() called with a negative 'duration' argument.\n", __FUNCTION__); + return; + } + + joystick.force_feedback_duration = duration; + joystick.force_feedback_duration_request = true; +} + +void wb_joystick_set_auto_centering_gain(double gain) { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return; + } + + joystick.auto_centering_gain = gain; + joystick.auto_centering_gain_request = true; +} + +void wb_joystick_set_resistance_gain(double gain) { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return; + } + + joystick.resistance_gain = gain; + joystick.resistance_gain_request = true; +} + +void wb_joystick_set_force_axis(int axis) { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return; + } + + if (axis >= joystick.number_of_axes) { + fprintf(stderr, "Error: %s() called with an 'axis' argument (%d) bigger than or equal to the number of axes (%d).\n", + __FUNCTION__, axis, joystick.number_of_axes); + return; + } + + joystick.force_axis = axis; + joystick.force_axis_request = true; +} + +bool wb_joystick_is_connected() { + if (joystick.sampling_period <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_joystick_enable().\n", __FUNCTION__); + return false; + } + + return joystick.connected; +} + +const char *wb_joystick_get_model() { + return joystick.model; +} diff --git a/src/controller/c/joystick_private.h b/src/controller/c/joystick_private.h new file mode 100644 index 0000000..2d5217e --- /dev/null +++ b/src/controller/c/joystick_private.h @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef JOYSTICK_PRIVATE_H +#define JOYSTICK_PRIVATE_H + +#include "request.h" + +void joystick_write_request(WbRequest *req); +void joystick_set_sampling_period(int sampling_period); +bool joystick_read_answer(int message, WbRequest *r); +void joystick_step_end(); +void wb_joystick_init(); + +#endif // JOYSTICK_PRIVATE_H diff --git a/src/controller/c/keyboard.c b/src/controller/c/keyboard.c new file mode 100644 index 0000000..7060d04 --- /dev/null +++ b/src/controller/c/keyboard.c @@ -0,0 +1,128 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "keyboard_private.h" +#include "messages.h" +#include "robot_private.h" + +#include // fprintf + +typedef struct { + int key[8]; + int sampling_period; + char pointer; +} WbKeyboard; + +static WbKeyboard keyboard; + +static void keyboard_read_value(WbRequest *r) { + int i; + int max = request_read_uchar(r); + for (i = 0; i < max; ++i) { + if (i >= 7) + request_read_uint32(r); // empty request + else + keyboard.key[i] = request_read_uint32(r); + } + if (max > 7) + max = 7; + keyboard.key[max] = -1; +} + +// Protected constants and funtions available from other files of the client library + +const int wb_KEYBOARD_END = WB_KEYBOARD_END, wb_KEYBOARD_HOME = WB_KEYBOARD_HOME, wb_KEYBOARD_LEFT = WB_KEYBOARD_LEFT, + wb_KEYBOARD_UP = WB_KEYBOARD_UP, wb_KEYBOARD_RIGHT = WB_KEYBOARD_RIGHT, wb_KEYBOARD_DOWN = WB_KEYBOARD_DOWN, + wb_KEYBOARD_PAGEUP = WB_KEYBOARD_PAGEUP, wb_KEYBOARD_PAGEDOWN = WB_KEYBOARD_PAGEDOWN, + wb_KEYBOARD_NUMPAD_HOME = WB_KEYBOARD_NUMPAD_HOME, wb_KEYBOARD_NUMPAD_LEFT = WB_KEYBOARD_NUMPAD_LEFT, + wb_KEYBOARD_NUMPAD_UP = WB_KEYBOARD_NUMPAD_UP, wb_KEYBOARD_NUMPAD_RIGHT = WB_KEYBOARD_NUMPAD_RIGHT, + wb_KEYBOARD_NUMPAD_DOWN = WB_KEYBOARD_NUMPAD_DOWN, wb_KEYBOARD_NUMPAD_END = WB_KEYBOARD_NUMPAD_END, + wb_KEYBOARD_KEY = WB_KEYBOARD_KEY, wb_KEYBOARD_SHIFT = WB_KEYBOARD_SHIFT, wb_KEYBOARD_CONTROL = WB_KEYBOARD_CONTROL, + wb_KEYBOARD_ALT = WB_KEYBOARD_ALT; + +void keyboard_write_request(WbRequest *req) { + if (keyboard.pointer == -1) { // need to enable or disable + request_write_uchar(req, C_ROBOT_SET_KEYBOARD_SAMPLING_PERIOD); + request_write_uint16(req, keyboard.sampling_period); + keyboard.pointer = 0; + } +} + +void keyboard_set_sampling_period(int sampling_period) { + keyboard.sampling_period = sampling_period; + keyboard.pointer = -1; +} + +bool keyboard_read_answer(int message, WbRequest *r) { + if (message == C_ROBOT_KEYBOARD_VALUE) { + keyboard_read_value(r); + return true; + } else + return false; +} + +void keyboard_step_end() { + if (keyboard.sampling_period) { + keyboard.key[0] = -1; + if (keyboard.pointer != -1) + keyboard.pointer = 0; + } +} + +void wb_keyboard_init() { + keyboard.sampling_period = 0; // initially disabled + keyboard.pointer = 0; + keyboard.key[0] = -1; +} + +// Public functions available from the keyboard API + +void wb_keyboard_enable(int sampling_period) { + robot_mutex_lock(); + keyboard.key[0] = -1; + keyboard.pointer = -1; // need to enable or disable + keyboard.sampling_period = sampling_period; + robot_mutex_unlock(); +} + +void wb_keyboard_disable() { + wb_keyboard_enable(0); +} + +int wb_keyboard_get_sampling_period() { + int sampling_period = 0; + robot_mutex_lock(); + sampling_period = keyboard.sampling_period; + robot_mutex_unlock(); + return sampling_period; +} + +int wb_keyboard_get_key() { + if (keyboard.sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_keyboard_enable().\n", __FUNCTION__); + + int r = -1; + robot_mutex_lock(); + if (keyboard.pointer != -1) { + r = keyboard.key[(int)keyboard.pointer]; + if (r >= 0) + keyboard.pointer++; + } + robot_mutex_unlock(); + return r; +} diff --git a/src/controller/c/keyboard_private.h b/src/controller/c/keyboard_private.h new file mode 100644 index 0000000..694c4a7 --- /dev/null +++ b/src/controller/c/keyboard_private.h @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef KEYBOARD_PRIVATE_H +#define KEYBOARD_PRIVATE_H + +#include "request.h" + +void keyboard_write_request(WbRequest *req); +void keyboard_set_sampling_period(int sampling_period); +bool keyboard_read_answer(int message, WbRequest *r); +void keyboard_step_end(); +void wb_keyboard_init(); + +#endif // KEYBOARD_PRIVATE_H diff --git a/src/controller/c/led.c b/src/controller/c/led.c new file mode 100644 index 0000000..da9564f --- /dev/null +++ b/src/controller/c/led.c @@ -0,0 +1,102 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +// *************************************************** +// this file contains the API code for the LED device +// *************************************************** + +#include +#include +#include +#include +#include +#include +#include "device_private.h" +#include "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool set_state; + int state; +} LED; + +static LED *led_create() { + LED *led = malloc(sizeof(LED)); + led->set_state = false; + led->state = 0; + return led; +} + +static LED *led_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_LED, true); + return d ? d->pdata : NULL; +} + +static void led_write_request(WbDevice *d, WbRequest *r) { + LED *led = d->pdata; + if (led->set_state) { + request_write_uchar(r, C_LED_SET); + request_write_int32(r, led->state); + led->set_state = false; + } +} + +static void led_cleanup(WbDevice *d) { + free(d->pdata); +} + +static void led_toggle_remote(WbDevice *d, WbRequest *r) { + LED *led = d->pdata; + if (led->state != 0) + led->set_state = true; +} + +// Exported functions + +void wb_led_init(WbDevice *d) { + d->read_answer = NULL; + d->write_request = led_write_request; + d->cleanup = led_cleanup; + d->pdata = led_create(); + d->toggle_remote = led_toggle_remote; +} + +// Public functions (available from the user API) + +void wb_led_set(WbDeviceTag tag, int value) { + robot_mutex_lock(); + LED *led = led_get_struct(tag); + if (led) { + led->state = value; + led->set_state = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +int wb_led_get(WbDeviceTag tag) { + int state = 0; + robot_mutex_lock(); + const LED *led = led_get_struct(tag); + if (led) + state = led->state; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return state; +} diff --git a/src/controller/c/lidar.c b/src/controller/c/lidar.c new file mode 100644 index 0000000..2862dc6 --- /dev/null +++ b/src/controller/c/lidar.c @@ -0,0 +1,516 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "abstract_camera.h" +#include "g_image.h" +#include "messages.h" +#include "remote_control_private.h" +#include "robot_private.h" + +typedef struct { + double max_range; + int number_of_layers; + int horizontal_resolution; + double frequency; + double min_frequency; + double max_frequency; + double vertical_fov; + bool point_cloud_enabled; + bool set_frequency; + bool set_enable_point_cloud; + bool set_disable_point_cloud; +} Lidar; + +static WbDevice *lidar_get_device(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_LIDAR, true); + return d; +} + +static AbstractCamera *lidar_get_abstract_camera_struct(WbDeviceTag t) { + WbDevice *d = lidar_get_device(t); + return d ? d->pdata : NULL; +} + +static Lidar *lidar_get_struct(WbDeviceTag t) { + AbstractCamera *ac = lidar_get_abstract_camera_struct(t); + return ac ? ac->pdata : NULL; +} + +static void wb_lidar_cleanup(WbDevice *d) { + AbstractCamera *ac = d->pdata; + if (ac == NULL) + return; + Lidar *l = ac->pdata; + if (l == NULL) + return; + free(l); + ac->pdata = NULL; + wb_abstract_camera_cleanup(d); +} + +static void wb_lidar_new(WbDevice *d, unsigned int id, int w, int h, double fov, double camnear, double max_range, bool planar, + int number_of_layers, double frequency, double min_frequency, double max_frequency, + double vertical_fov, int horizontal_resolution) { + Lidar *l; + wb_lidar_cleanup(d); + wb_abstract_camera_new(d, id, w, h, fov, camnear, planar); + + l = malloc(sizeof(Lidar)); + l->max_range = max_range; + l->number_of_layers = number_of_layers; + l->horizontal_resolution = horizontal_resolution; + l->frequency = frequency; + l->min_frequency = min_frequency; + l->max_frequency = max_frequency; + l->vertical_fov = vertical_fov; + l->point_cloud_enabled = false; + l->set_frequency = false; + l->set_enable_point_cloud = false; + l->set_disable_point_cloud = false; + + AbstractCamera *ac = d->pdata; + ac->pdata = l; +} + +static void wb_lidar_write_request(WbDevice *d, WbRequest *r) { + wb_abstract_camera_write_request(d, r); + AbstractCamera *ac = d->pdata; + Lidar *l = ac->pdata; + if (l->set_frequency) { + request_write_uchar(r, C_LIDAR_SET_FREQUENCY); + request_write_double(r, l->frequency); + l->set_frequency = false; // done + } + if (l->set_enable_point_cloud) { + request_write_uchar(r, C_LIDAR_ENABLE_POINT_CLOUD); + l->set_enable_point_cloud = false; // done + } + if (l->set_disable_point_cloud) { + request_write_uchar(r, C_LIDAR_DISABLE_POINT_CLOUD); + l->set_disable_point_cloud = false; // done + } +} + +static void wb_lidar_read_answer(WbDevice *d, WbRequest *r) { + unsigned char command = request_read_uchar(r); + if (wb_abstract_camera_handle_command(d, r, command)) + return; + unsigned int uid; + int width, height, number_of_layers, horizontal_resolution; + double fov, camnear, max_range, frequency, min_frequency, max_frequency, vertical_fov; + bool planar; + + AbstractCamera *ac = d->pdata; + Lidar *l = NULL; + + switch (command) { + case C_CONFIGURE: + uid = request_read_uint32(r); + width = request_read_uint16(r); + height = request_read_uint16(r); + fov = request_read_double(r); + camnear = request_read_double(r); + planar = request_read_uchar(r); + max_range = request_read_double(r); + number_of_layers = request_read_uint16(r); + frequency = request_read_double(r); + min_frequency = request_read_double(r); + max_frequency = request_read_double(r); + vertical_fov = request_read_double(r); + horizontal_resolution = request_read_double(r); + + // printf("new lidar %u %d %d %lf %lf %lf %d\n", uid, width, height, fov, camnear, max_range, planar); + wb_lidar_new(d, uid, width, height, fov, camnear, max_range, planar, number_of_layers, frequency, min_frequency, + max_frequency, vertical_fov, horizontal_resolution); + break; + case C_CAMERA_RECONFIGURE: + l = ac->pdata; + ac->fov = request_read_double(r); + ac->camnear = request_read_double(r); + ac->planar = request_read_uchar(r); + l->max_range = request_read_double(r); + l->number_of_layers = request_read_uint16(r); + l->frequency = request_read_double(r); + l->min_frequency = request_read_double(r); + l->max_frequency = request_read_double(r); + l->vertical_fov = request_read_double(r); + l->horizontal_resolution = request_read_double(r); + break; + + default: + ROBOT_ASSERT(0); + break; + } +} + +static void lidar_toggle_remote(WbDevice *d, WbRequest *r) { + abstract_camera_toggle_remote(d, r); + AbstractCamera *ac = d->pdata; + Lidar *l = ac->pdata; + if (ac->sampling_period != 0) { + ac->enable = true; + if (remote_control_is_function_defined("wbr_lidar_set_frequency")) + l->set_frequency = true; + } +} + +// Protected functions available from other source files + +void wb_lidar_init(WbDevice *d) { + d->read_answer = wb_lidar_read_answer; + d->write_request = wb_lidar_write_request; + d->cleanup = wb_lidar_cleanup; + d->pdata = NULL; + d->toggle_remote = lidar_toggle_remote; + // g_print("lidar init done\n"); +} + +int wb_lidar_get_unique_id(const WbDevice *d) { + const AbstractCamera *ac = d->pdata; + return ac->unique_id; +} + +void wbr_lidar_set_image(WbDeviceTag t, const unsigned char *image) { + WbDevice *d = lidar_get_device(t); + if (d) + wbr_abstract_camera_set_image(d, image); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +unsigned char *wbr_lidar_get_image_buffer(WbDeviceTag t) { + WbDevice *d = lidar_get_device(t); + if (d) + return wbr_abstract_camera_get_image_buffer(d); + + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return (unsigned char *)""; // don't return NULL, swig can't create string objects from NULL +} + +// Public functions available from the lidar API + +void wb_lidar_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + WbDevice *d = lidar_get_device(tag); + if (d) + wb_abstract_camera_enable(d, sampling_period); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +void wb_lidar_enable_point_cloud(WbDeviceTag tag) { + robot_mutex_lock(); + Lidar *l = lidar_get_struct(tag); + if (!l) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + else { + l->point_cloud_enabled = true; + l->set_enable_point_cloud = true; + l->set_disable_point_cloud = false; + } + robot_mutex_unlock(); +} + +void wb_lidar_disable(WbDeviceTag tag) { + const Lidar *l = lidar_get_struct(tag); + if (!l) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + else + wb_lidar_enable(tag, 0); +} + +void wb_lidar_disable_point_cloud(WbDeviceTag tag) { + robot_mutex_lock(); + Lidar *l = lidar_get_struct(tag); + if (!l) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + else { + l->point_cloud_enabled = false; + l->set_disable_point_cloud = true; + l->set_enable_point_cloud = false; + } + robot_mutex_unlock(); +} + +int wb_lidar_get_sampling_period(WbDeviceTag tag) { + const Lidar *l = lidar_get_struct(tag); + if (!l) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + + return wb_abstract_camera_get_sampling_period(lidar_get_device(tag)); +} + +bool wb_lidar_is_point_cloud_enabled(WbDeviceTag tag) { + bool result = false; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->point_cloud_enabled; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +int wb_lidar_get_number_of_layers(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->number_of_layers; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_lidar_get_min_frequency(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->min_frequency; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_lidar_get_max_frequency(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->max_frequency; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_lidar_get_frequency(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->frequency; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +void wb_lidar_set_frequency(WbDeviceTag tag, double frequency) { + robot_mutex_lock(); + Lidar *l = lidar_get_struct(tag); + if (!l) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + else { + if (frequency < l->min_frequency || frequency > l->max_frequency) + fprintf(stderr, "Error: %s() out of frequency range [%f, %f].\n", __FUNCTION__, l->min_frequency, l->max_frequency); + else { + l->frequency = frequency; + l->set_frequency = true; + } + } + robot_mutex_unlock(); +} + +int wb_lidar_get_horizontal_resolution(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->horizontal_resolution; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_lidar_get_fov(WbDeviceTag tag) { + const WbDevice *d = lidar_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_fov(d); +} + +double wb_lidar_get_vertical_fov(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->vertical_fov; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +double wb_lidar_get_min_range(WbDeviceTag tag) { + const WbDevice *d = lidar_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return wb_abstract_camera_get_near(d); +} + +double wb_lidar_get_max_range(WbDeviceTag tag) { + double result = NAN; + robot_mutex_lock(); + const Lidar *l = lidar_get_struct(tag); + if (l) + result = l->max_range; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const float *wb_lidar_get_range_image(WbDeviceTag tag) { + robot_mutex_lock(); + AbstractCamera *ac = lidar_get_abstract_camera_struct(tag); + + if (!ac) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return NULL; + } + + if (wb_robot_get_mode() == WB_MODE_REMOTE_CONTROL) { + robot_mutex_unlock(); + return (const float *)(void *)ac->image->data; + } + + if (ac->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_lidar_enable().\n", __FUNCTION__); + + robot_mutex_unlock(); + + return (const float *)(void *)ac->image->data; +} + +const float *wb_lidar_get_layer_range_image(WbDeviceTag tag, int layer) { + const Lidar *l = lidar_get_struct(tag); + if (!l) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; + } + + if (wb_abstract_camera_get_sampling_period(lidar_get_device(tag)) <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_lidar_enable().\n", __FUNCTION__); + return NULL; + } + + if (layer >= l->number_of_layers) { + fprintf(stderr, + "Error: %s() called with a 'layer' argument (%d) bigger or equal to the number of layers of this lidar (%d).\n", + __FUNCTION__, layer, l->number_of_layers); + return NULL; + } else if (layer < 0) { + fprintf(stderr, "Error: %s() called with a negative 'layer' argument.\n", __FUNCTION__); + return NULL; + } + const float *image = wb_lidar_get_range_image(tag); + if (image == NULL) + return NULL; + return image + layer * l->horizontal_resolution; +} + +const WbLidarPoint *wb_lidar_get_point_cloud(WbDeviceTag tag) { + const Lidar *l = lidar_get_struct(tag); + if (!l) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; + } + if (!l->point_cloud_enabled) { + fprintf(stderr, "Error: %s() called for a lidar with point cloud disabled. Please use: wb_lidar_enable_point_cloud().\n", + __FUNCTION__); + return NULL; + } + if (wb_abstract_camera_get_sampling_period(lidar_get_device(tag)) <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_lidar_enable().\n", __FUNCTION__); + return NULL; + } + const float *image = wb_lidar_get_range_image(tag); + if (image == NULL) + return NULL; + return (WbLidarPoint *)(image + l->number_of_layers * l->horizontal_resolution); +} + +const WbLidarPoint *wb_lidar_get_layer_point_cloud(WbDeviceTag tag, int layer) { + const Lidar *l = lidar_get_struct(tag); + if (!l) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; + } + if (!l->point_cloud_enabled) { + fprintf(stderr, "Error: %s() called for a lidar with point cloud disabled. Please use: wb_lidar_enable_point_cloud().\n", + __FUNCTION__); + return NULL; + } + if (wb_abstract_camera_get_sampling_period(lidar_get_device(tag)) <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_lidar_enable().\n", __FUNCTION__); + return 0; + } + if (layer >= l->number_of_layers) { + fprintf(stderr, + "Error: %s() called with a 'layer' argument (%d) bigger or equal to the number of layers of this lidar (%d).\n", + __FUNCTION__, layer, l->number_of_layers); + return NULL; + } else if (layer < 0) { + fprintf(stderr, "Error: %s() called with a negative 'layer' argument.\n", __FUNCTION__); + return NULL; + } + const WbLidarPoint *point_cloud = wb_lidar_get_point_cloud(tag); + if (point_cloud == NULL) + return NULL; + return point_cloud + layer * l->horizontal_resolution; +} + +int wb_lidar_get_number_of_points(WbDeviceTag tag) { + const Lidar *l = lidar_get_struct(tag); + if (!l) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return 0; + } + if (!l->point_cloud_enabled) { + fprintf(stderr, "Error: %s() called for a lidar with point cloud disabled! Please use: wb_lidar_enable_point_cloud().\n", + __FUNCTION__); + return 0; + } + if (wb_abstract_camera_get_sampling_period(lidar_get_device(tag)) <= 0) { + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_lidar_enable().\n", __FUNCTION__); + return 0; + } + return l->horizontal_resolution * l->number_of_layers; +} + +const WbLidarPoint *wb_lidar_get_point(WbDeviceTag tag, int index) { + const Lidar *l = lidar_get_struct(tag); + if (l) + return (wb_lidar_get_point_cloud(tag) + index); + + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return NULL; +} diff --git a/src/controller/c/light_sensor.c b/src/controller/c/light_sensor.c new file mode 100644 index 0000000..0906ece --- /dev/null +++ b/src/controller/c/light_sensor.c @@ -0,0 +1,186 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +// Static functions + +typedef struct { + bool enable; // need to enable device ? + int sampling_period; // milliseconds + double value; + int lookup_table_size; + double *lookup_table; +} LightSensor; + +static LightSensor *light_sensor_create() { + LightSensor *ls = malloc(sizeof(LightSensor)); + ls->enable = false; + ls->sampling_period = 0; + ls->value = NAN; + ls->lookup_table = NULL; + ls->lookup_table_size = 0; + return ls; +} + +static LightSensor *light_sensor_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_LIGHT_SENSOR, true); + return d ? d->pdata : NULL; +} + +static void light_sensor_read_answer(WbDevice *d, WbRequest *r) { + LightSensor *ls = (LightSensor *)d->pdata; + switch (request_read_uchar(r)) { + case C_LIGHT_SENSOR_DATA: + ls->value = request_read_double(r); + break; + case C_CONFIGURE: + ls->lookup_table_size = request_read_int32(r); + free(ls->lookup_table); + ls->lookup_table = NULL; + if (ls->lookup_table_size > 0) { + ls->lookup_table = (double *)malloc(sizeof(double) * ls->lookup_table_size * 3); + for (int i = 0; i < ls->lookup_table_size * 3; i++) + ls->lookup_table[i] = request_read_double(r); + } + break; + default: + ROBOT_ASSERT(0); // should never be reached + break; + } +} + +int wb_light_sensor_get_lookup_table_size(WbDeviceTag tag) { + int result = 0; + robot_mutex_lock(); + const LightSensor *dev = light_sensor_get_struct(tag); + if (dev) + result = dev->lookup_table_size; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +const double *wb_light_sensor_get_lookup_table(WbDeviceTag tag) { + double *result = NULL; + robot_mutex_lock(); + LightSensor *dev = light_sensor_get_struct(tag); + if (dev) + result = dev->lookup_table; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +static void light_sensor_write_request(WbDevice *d, WbRequest *r) { + LightSensor *ls = (LightSensor *)d->pdata; + if (ls->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, ls->sampling_period); + ls->enable = false; // done + } +} + +static void light_sensor_cleanup(WbDevice *d) { + LightSensor *ls = (LightSensor *)d->pdata; + free(ls->lookup_table); + free(d->pdata); +} + +static void light_sensor_toggle_remote(WbDevice *d, WbRequest *r) { + LightSensor *ls = (LightSensor *)d->pdata; + if (ls->sampling_period != 0) + ls->enable = true; +} + +void wbr_light_sensor_set_value(WbDeviceTag t, double value) { + LightSensor *ls = light_sensor_get_struct(t); + if (ls) { + ls->value = value; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Protected functions (exported to device.cc) + +void wb_light_sensor_init(WbDevice *d) { + d->pdata = light_sensor_create(); + d->write_request = light_sensor_write_request; + d->read_answer = light_sensor_read_answer; + d->cleanup = light_sensor_cleanup; + d->toggle_remote = light_sensor_toggle_remote; +} + +// Public function available from the user API + +void wb_light_sensor_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + LightSensor *ls = light_sensor_get_struct(tag); + if (ls) { + ls->sampling_period = sampling_period; + ls->enable = true; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_light_sensor_disable(WbDeviceTag tag) { + const LightSensor *ls = light_sensor_get_struct(tag); + if (ls) + wb_light_sensor_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +int wb_light_sensor_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const LightSensor *ls = light_sensor_get_struct(tag); + if (ls) + sampling_period = ls->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +double wb_light_sensor_get_value(WbDeviceTag tag) { + double value = NAN; + robot_mutex_lock(); + const LightSensor *ls = light_sensor_get_struct(tag); + if (ls) { + if (ls->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_light_sensor_enable().\n", __FUNCTION__); + value = ls->value; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return value; +} diff --git a/src/controller/c/messages.h b/src/controller/c/messages.h new file mode 100644 index 0000000..4eddad8 --- /dev/null +++ b/src/controller/c/messages.h @@ -0,0 +1,310 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +#ifndef MESSAGES_H +#define MESSAGES_H + +// for any device + +// both directions +#define C_CONFIGURE 0 + +// ctr -> sim +#define C_SET_SAMPLING_PERIOD 1 + +// sim -> ctr +#define TCP_DATA_TYPE 0 +#define TCP_IMAGE_TYPE 1 + +// for the root device (robot) + +// sim -> ctr +#define C_ROBOT_QUIT 2 +#define C_ROBOT_JOYSTICK_CONFIG 3 +#define C_ROBOT_TIME 4 +#define C_ROBOT_BATTERY_VALUE 5 +#define C_ROBOT_KEYBOARD_VALUE 6 +#define C_ROBOT_JOYSTICK_VALUE 7 +#define C_ROBOT_MOUSE_VALUE 8 +#define C_ROBOT_WINDOW_SHOW 9 +#define C_ROBOT_SIMULATION_CHANGE_MODE 10 +#define C_ROBOT_DATA 11 +#define C_ROBOT_SUPERVISOR 12 +#define C_ROBOT_MODEL 13 +#define C_ROBOT_WINDOW_UPDATE 14 +#define C_ROBOT_NEW_DEVICE 15 +#define C_SUPERVISOR_ANIMATION_START_STATUS 16 +#define C_SUPERVISOR_ANIMATION_STOP_STATUS 17 +#define C_SUPERVISOR_MOVIE_STATUS 18 +#define C_SUPERVISOR_NODE_REGENERATED 19 +#define C_SUPERVISOR_FIELD_COUNT_CHANGED 20 + +// ctr -> sim +#define C_CONSOLE_MESSAGE 30 +#define C_ROBOT_SET_BATTERY_SAMPLING_PERIOD 31 +#define C_ROBOT_REMOTE_ON 32 +#define C_ROBOT_REMOTE_OFF 33 +#define C_ROBOT_SET_KEYBOARD_SAMPLING_PERIOD 34 +#define C_ROBOT_SET_JOYSTICK_SAMPLING_PERIOD 35 +#define C_ROBOT_SET_MOUSE_SAMPLING_PERIOD 36 +#define C_ROBOT_SET_DATA 37 +#define C_ROBOT_CLIENT_EXIT_NOTIFY 38 +#define C_ROBOT_MOUSE_ENABLE_3D_POSITION 39 +#define C_ROBOT_PIN 40 +#define C_ROBOT_SET_JOYSTICK_FORCE_FEEDBACK 41 +#define C_ROBOT_SET_JOYSTICK_FORCE_FEEDBACK_DURATION 42 +#define C_ROBOT_SET_JOYSTICK_AUTO_CENTERING_GAIN 43 +#define C_ROBOT_SET_JOYSTICK_RESISTANCE_GAIN 44 +#define C_ROBOT_SET_JOYSTICK_FORCE_AXIS 45 +#define C_ROBOT_URDF 46 +#define C_SUPERVISOR_EXPORT_IMAGE 50 +#define C_SUPERVISOR_LOAD_WORLD 51 +#define C_SUPERVISOR_RELOAD_WORLD 52 +#define C_SUPERVISOR_SET_LABEL 53 +#define C_SUPERVISOR_SIMULATION_QUIT 54 +#define C_SUPERVISOR_SIMULATION_RESET 55 +#define C_SUPERVISOR_SIMULATION_CHANGE_MODE 56 +#define C_SUPERVISOR_SIMULATION_RESET_PHYSICS 57 +#define C_SUPERVISOR_START_MOVIE 58 +#define C_SUPERVISOR_STOP_MOVIE 59 +#define C_SUPERVISOR_START_ANIMATION 60 +#define C_SUPERVISOR_STOP_ANIMATION 61 +#define C_SUPERVISOR_FIELD_SET_VALUE 62 +#define C_SUPERVISOR_FIELD_REMOVE_VALUE 63 +#define C_SUPERVISOR_FIELD_GET_FROM_INDEX 64 +#define C_SUPERVISOR_NODE_GET_FIELD_COUNT 65 +#define C_SUPERVISOR_NODE_SET_VELOCITY 66 +#define C_SUPERVISOR_NODE_RESET_PHYSICS 67 +#define C_SUPERVISOR_NODE_RESTART_CONTROLLER 68 +#define C_SUPERVISOR_NODE_SET_VISIBILITY 69 +#define C_SUPERVISOR_NODE_MOVE_VIEWPOINT 70 +#define C_SUPERVISOR_NODE_ADD_FORCE 71 +#define C_SUPERVISOR_NODE_ADD_FORCE_WITH_OFFSET 72 +#define C_SUPERVISOR_NODE_ADD_TORQUE 73 +#define C_SUPERVISOR_NODE_SAVE_STATE 74 +#define C_SUPERVISOR_NODE_RESET_STATE 75 +#define C_SUPERVISOR_NODE_SET_JOINT_POSITION 76 +#define C_SUPERVISOR_NODE_EXPORT_STRING 77 +#define C_SUPERVISOR_NODE_GET_PROTO 78 + +// ctr <-> sim +#define C_ROBOT_WAIT_FOR_USER_INPUT_EVENT 80 +#define C_ROBOT_WWI_MESSAGE 81 +#define C_SUPERVISOR_SAVE_WORLD 82 +#define C_SUPERVISOR_NODE_GET_FROM_ID 83 +#define C_SUPERVISOR_NODE_GET_FROM_DEF 84 +#define C_SUPERVISOR_NODE_GET_FROM_TAG 85 +#define C_SUPERVISOR_NODE_GET_SELECTED 86 +#define C_SUPERVISOR_FIELD_GET_FROM_NAME 87 +#define C_SUPERVISOR_FIELD_GET_VALUE 88 +#define C_SUPERVISOR_FIELD_INSERT_VALUE 89 +#define C_SUPERVISOR_NODE_GET_POSITION 90 +#define C_SUPERVISOR_NODE_GET_ORIENTATION 91 +#define C_SUPERVISOR_NODE_GET_POSE 92 +#define C_SUPERVISOR_NODE_GET_CENTER_OF_MASS 93 +#define C_SUPERVISOR_NODE_GET_CONTACT_POINTS 94 +#define C_SUPERVISOR_NODE_GET_STATIC_BALANCE 95 +#define C_SUPERVISOR_NODE_GET_VELOCITY 96 +#define C_SUPERVISOR_NODE_REMOVE_NODE 97 +#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_IS_USED 98 +#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_POSITION 99 +#define C_SUPERVISOR_VIRTUAL_REALITY_HEADSET_GET_ORIENTATION 100 +#define C_SUPERVISOR_FIELD_CHANGE_TRACKING_STATE 101 +#define C_SUPERVISOR_POSE_CHANGE_TRACKING_STATE 102 +#define C_SUPERVISOR_CONTACT_POINTS_CHANGE_TRACKING_STATE 103 + +// for the abstract camera device +// sim -> ctr +#define C_ABSTRACT_CAMERA_SERIAL_IMAGE 0 + +// for the camera device +// ctr -> sim +#define C_CAMERA_SET_FOV 4 +#define C_CAMERA_SET_FOCAL 5 +#define C_CAMERA_SET_EXPOSURE 6 +// sim -> ctr +#define C_CAMERA_RECONFIGURE 7 +#define C_CAMERA_MEMORY_MAPPED_FILE 8 +#define C_CAMERA_SERIAL_SEGMENTATION_IMAGE 9 + +// for the camera recognition +// ctr -> sim +#define C_CAMERA_SET_RECOGNITION_SAMPLING_PERIOD 10 +#define C_CAMERA_ENABLE_SEGMENTATION 11 +// sim -> ctr +#define C_CAMERA_OBJECTS 12 +#define C_CAMERA_SEGMENTATION_MEMORY_MAPPED_FILE 13 +#define C_CAMERA_SET_SEGMENTATION 14 + +// for the emitter device +// ctr -> sim +#define C_EMITTER_SEND 0 +// ctr <-> sim +#define C_EMITTER_SET_CHANNEL 1 +#define C_EMITTER_SET_RANGE 2 +// sim -> ctr +#define C_EMITTER_SET_BUFFER_SIZE 3 +#define C_EMITTER_SET_ALLOWED_CHANNELS 4 + +// for the receiver device +// ctr -> sim +#define C_RECEIVER_RECEIVE 1 +#define C_RECEIVER_SET_CHANNEL 2 + +// for the skin device +// ctr -> sim +#define C_SKIN_SET_BONE_ORIENTATION 1 +#define C_SKIN_SET_BONE_POSITION 2 +// ctr <-> sim +#define C_SKIN_GET_BONE_ORIENTATION 3 +#define C_SKIN_GET_BONE_POSITION 4 + +// for the motor device +// ctr -> sim +#define C_MOTOR_SET_POSITION 1 +#define C_MOTOR_SET_VELOCITY 2 +#define C_MOTOR_SET_AVAILABLE_FORCE 3 +#define C_MOTOR_SET_FORCE 4 +#define C_MOTOR_FEEDBACK 5 +#define C_MOTOR_SET_ACCELERATION 6 +#define C_MOTOR_SET_CONTROL_PID 7 +// ctr <-> sim +#define C_MOTOR_GET_ASSOCIATED_DEVICE 8 + +// for the brake device +// ctr -> sim, to be ORed +#define C_BRAKE_SET_DAMPING_CONSTANT 2 +// ctr <-> sim, to be ORed +#define C_BRAKE_GET_ASSOCIATED_DEVICE 4 + +// for the led device +// ctr -> sim +#define C_LED_SET 1 + +// for the display device +// ctr -> sim +#define C_DISPLAY_SET_COLOR 16 +#define C_DISPLAY_SET_ALPHA 17 +#define C_DISPLAY_SET_OPACITY 18 +#define C_DISPLAY_SET_FONT 19 +#define C_DISPLAY_ATTACH_CAMERA 20 +#define C_DISPLAY_DETACH_CAMERA 21 +#define C_DISPLAY_DRAW_PIXEL 32 +#define C_DISPLAY_DRAW_LINE 33 +#define C_DISPLAY_DRAW_TEXT 34 +#define C_DISPLAY_DRAW_RECTANGLE 35 +#define C_DISPLAY_DRAW_OVAL 36 +#define C_DISPLAY_DRAW_POLYGON 37 +#define C_DISPLAY_IMAGE_COPY 64 +#define C_DISPLAY_IMAGE_PASTE 65 +#define C_DISPLAY_IMAGE_SAVE 66 +#define C_DISPLAY_IMAGE_LOAD 67 +#define C_DISPLAY_IMAGE_DELETE 68 +#define C_DISPLAY_IMAGE_GET_ALL 69 + +// for the camera device +// ctr -> sim +#define C_GPS_DATA 2 + +// for the lidar device +// ctr -> sim +#define C_LIDAR_SET_FREQUENCY 16 +#define C_LIDAR_ENABLE_POINT_CLOUD 17 +#define C_LIDAR_DISABLE_POINT_CLOUD 18 + +// for the pen device +// ctr-> sim +#define C_PEN_WRITE 1 +#define C_PEN_DONT_WRITE 2 +#define C_PEN_SET_INK_COLOR 4 + +// for the position sensor device +// sim -> ctr +#define C_POSITION_SENSOR_DATA 1 +// ctr <-> sim +#define C_POSITION_SENSOR_GET_ASSOCIATED_DEVICE 2 + +// for the radar device +// sim -> ctr +#define C_RADAR_DATA 1 + +// for the radio device +// ctr -> sim +#define C_RADIO_SEND 2 +#define C_RADIO_SET_ADDRESS 3 +#define C_RADIO_SET_FREQUENCY 4 +#define C_RADIO_SET_CHANNEL 5 +#define C_RADIO_SET_BITRATE 6 +#define C_RADIO_SET_RX_SENSITIVITY 7 +#define C_RADIO_SET_TX_POWER 8 + +// sim -> ctr +#define C_RADIO_RECEIVE 1 + +// connector device +#define C_CONNECTOR_GET_PRESENCE 1 +#define C_CONNECTOR_LOCK 2 +#define C_CONNECTOR_UNLOCK 3 + +// DistanceSensor device +#define C_DISTANCE_SENSOR_DATA 50 + +// Accelerometer device +#define C_ACCELEROMETER_DATA 51 + +// Compass device +#define C_COMPASS_DATA 52 + +// Gyro device +#define C_GYRO_DATA 53 + +// Inertial unit device +#define C_INERTIAL_UNIT_DATA 54 + +// LightSensor device +#define C_LIGHT_SENSOR_DATA 55 + +// TouchSensor device +#define C_TOUCH_SENSOR_DATA 40 +#define C_TOUCH_SENSOR_DATA_3D 41 + +// VacuumGripper device +#define C_VACUUM_GRIPPER_GET_PRESENCE 1 +#define C_VACUUM_GRIPPER_TURN_ON 2 +#define C_VACUUM_GRIPPER_TURN_OFF 3 + +// Speaker device +// ctr -> sim +#define C_SPEAKER_PLAY_SOUND 1 +#define C_SPEAKER_STOP 2 +#define C_SPEAKER_SET_ENGINE 3 +#define C_SPEAKER_SET_LANGUAGE 4 +#define C_SPEAKER_SPEAK 5 +// sim -> ctr +#define C_SPEAKER_SOUND_OVER 6 +#define C_SPEAKER_SPEAK_OVER 7 + +// Propeller device +// ctr -> sim +#define C_PROPELLER_SET_TORQUE 1 + +// Microphone device +#define C_MICROPHONE_RECEIVE 1 + +// Altimeter device +// ctr -> sim +#define C_ALTIMETER_DATA 1 +#endif // MESSAGES_H diff --git a/src/controller/c/microphone.c b/src/controller/c/microphone.c new file mode 100644 index 0000000..3654704 --- /dev/null +++ b/src/controller/c/microphone.c @@ -0,0 +1,181 @@ +/* + * Copyright 1996-2024 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 + * + * https://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 "device_private.h" +#include "messages.h" +#include "robot_private.h" + +typedef struct { + int enable : 1; // need to enable device ? + int sampling_period; // milliseconds + double aperture; + double sensitivity; + void *sample; // received sample + int sample_size; // received sample size +} Microphone; + +static Microphone *microphone_create() { + Microphone *mic = malloc(sizeof(Microphone)); + mic->enable = 0; + mic->sampling_period = 0; + mic->aperture = -1.0; + mic->sensitivity = -1.0; + mic->sample = NULL; + return mic; +} + +static void microphone_destroy(Microphone *mic) { + free(mic->sample); + free(mic); +} + +// Static functions + +static inline Microphone *microphone_get_struct(WbDeviceTag t) { + WbDevice *d = robot_get_device_with_node(t, WB_NODE_MICROPHONE, true); + return d ? d->pdata : NULL; +} + +static void microphone_read_answer(WbDevice *d, WbRequest *r) { + Microphone *mic = (Microphone *)d->pdata; + switch (request_read_uchar(r)) { + case C_CONFIGURE: + mic->aperture = request_read_double(r); + mic->sensitivity = request_read_double(r); + break; + + case C_MICROPHONE_RECEIVE: + mic->sample_size = request_read_int32(r); + free(mic->sample); + mic->sample = malloc(mic->sample_size); + memcpy(mic->sample, request_read_data(r, mic->sample_size), mic->sample_size); + break; + + default: + ROBOT_ASSERT(0); + } +} + +static void microphone_write_request(WbDevice *d, WbRequest *r) { + Microphone *mic = (Microphone *)d->pdata; + if (mic->enable) { + request_write_uchar(r, C_SET_SAMPLING_PERIOD); + request_write_uint16(r, mic->sampling_period); + mic->enable = 0; // done + } +} + +static void microphone_cleanup(WbDevice *d) { + microphone_destroy((Microphone *)d->pdata); +} + +int wb_microphone_get_sampling_period(WbDeviceTag tag) { + int sampling_period = 0; + robot_mutex_lock(); + const Microphone *mic = microphone_get_struct(tag); + if (mic) + sampling_period = mic->sampling_period; + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return sampling_period; +} + +static void microphone_toggle_remote(WbDevice *d, WbRequest *r) { + Microphone *mic = (Microphone *)d->pdata; + if (mic->sampling_period != 0) + mic->enable = 1; +} + +void wbr_microphone_set_buffer(WbDeviceTag t, const unsigned char *buffer, int size) { + Microphone *mic = microphone_get_struct(t); + if (mic) { + mic->sample_size = size; + free(mic->sample); + mic->sample = malloc(mic->sample_size); + memcpy(mic->sample, buffer, mic->sample_size); + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +// Exported functions + +void wb_microphone_init(WbDevice *d) { + d->read_answer = microphone_read_answer; + d->write_request = microphone_write_request; + d->cleanup = microphone_cleanup; + d->pdata = microphone_create(); + d->toggle_remote = microphone_toggle_remote; +} + +// Public functions available from the user API + +void wb_microphone_enable(WbDeviceTag tag, int sampling_period) { + if (sampling_period < 0) { + fprintf(stderr, "Error: %s() called with negative sampling period.\n", __FUNCTION__); + return; + } + + robot_mutex_lock(); + Microphone *mic = microphone_get_struct(tag); + if (mic) { + mic->enable = 1; + mic->sampling_period = sampling_period; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); +} + +void wb_microphone_disable(WbDeviceTag tag) { + const Microphone *mic = microphone_get_struct(tag); + if (mic) + wb_microphone_enable(tag, 0); + else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); +} + +const void *wb_microphone_get_sample_data(WbDeviceTag tag) { + const void *result = NULL; + robot_mutex_lock(); + const Microphone *mic = microphone_get_struct(tag); + if (mic) { + if (mic->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_microphone_enable().\n", __FUNCTION__); + result = mic->sample; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} + +int wb_microphone_get_sample_size(WbDeviceTag tag) { + int result = -1; + robot_mutex_lock(); + const Microphone *mic = microphone_get_struct(tag); + if (mic) { + if (mic->sampling_period <= 0) + fprintf(stderr, "Error: %s() called for a disabled device! Please use: wb_microphone_enable().\n", __FUNCTION__); + result = mic->sample_size; + } else + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + robot_mutex_unlock(); + return result; +} diff --git a/src/controller/c/motion.c b/src/controller/c/motion.c new file mode 100644 index 0000000..02c5e62 --- /dev/null +++ b/src/controller/c/motion.c @@ -0,0 +1,564 @@ +/* + * Copyright 1996-2024 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 + * + * https://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. + */ + +//----------------------------------------------------------- +// Purpose: Webots API for loading and playing .motion files +//----------------------------------------------------------- + +#include +#include +#include +#include +#include +#include +#include "robot_private.h" + +typedef struct WbMotionStructPrivate { + int n_joints; // number of joints (columns) in the file + int n_poses; // number of poses (lines) in the file + char *filename; // file name + char **joint_names; // array of n_joints joint names + WbDeviceTag *tags; // array of n_joints device tags + WbNodeType *types; // array of n_joints device types + int *times; // array of n_poses poses + double **pos; // two-dimensional [n_poses][n_joints] array + int elapsed; // elapsed time when playing this motion file + bool playing; // is currenly playing + bool reverse; // playing forward of backwards ? + bool loop; // loop when reaching the end (or begining) ? + WbMotionRef next; // next struct in list +} WbMotionStruct; + +extern void wb_motor_set_position_no_mutex(WbDeviceTag, double); + +static const int UNDEFINED_TIME = -1; +static const int MAX_LINE = 4096; +static const double UNDEFINED_POSITION = -9999999.9; +static WbMotionRef head = NULL; +static const char *HEADER = "#WEBOTS_MOTION"; +static const char *VERSION = "V1.0"; +static int cleanup_done = 0; + +// string to time conversion +// acceptable input format: [[Minutes:]Seconds:]:Milliseconds +// returns UNDEFINED_TIME in case of syntax error +static int str_to_time(const char *token) { + // check for illegal characters + const char cset[] = "0123456789:"; + if (strspn(token, cset) < strlen(token)) + return UNDEFINED_TIME; + + // count number of colons + int k = 0; + const char *p = token; + while (*p) { + if (*p == ':') + k++; + p++; + } + + int v[3] = {0, 0, 0}; + int r; + switch (k) { + case 0: + r = sscanf(token, "%d", &v[2]); + break; + case 1: + r = sscanf(token, "%d:%d", &v[1], &v[2]); + break; + case 2: + r = sscanf(token, "%d:%d:%d", &v[0], &v[1], &v[2]); + break; + default: + return UNDEFINED_TIME; + } + + // unexpected number of items + if (r != k + 1) + return UNDEFINED_TIME; + + return v[0] * 60000 + v[1] * 1000 + v[2]; +} + +static char *next_token(char *buffer) { + return strtok(buffer, ",\n\r"); +} + +// check the file syntax and find out the number of joints and poses +// returns true only if the syntax is 100% correct +static bool motion_check_file(FILE *file, const char *filename, int *n_joints, int *n_poses) { + // line buffer and counter + char buffer[MAX_LINE]; + int line = 1; + int joints = 0; + + if (!file || fgets(buffer, MAX_LINE, file) == NULL) { + fprintf(stderr, "Error: wbu_motion_new(): file '%s' is empty.\n", filename); + return false; + } + + const char *token = next_token(buffer); + if (!token) { + fprintf(stderr, "Error: wbu_motion_new(): unexpected end of file '%s'.\n", filename); + return false; + } + + if (strcmp(token, HEADER) != 0) { + fprintf(stderr, "Error: wbu_motion_new(): invalid motion file header in file '%s'.\n", filename); + return false; + } + + token = next_token(NULL); + if (!token) { + fprintf(stderr, "Error: wbu_motion_new(): unexpected end of file '%s'.\n", filename); + return false; + } + + if (strcmp(token, VERSION) != 0) { + fprintf(stderr, "Error: wbu_motion_new(): unsupported version number '%s' in motion file '%s'.\n", token, filename); + return false; + } + + // count joints (columns) + token = next_token(NULL); + while (token) { + joints++; + token = next_token(NULL); + } + + int time = -1; + + // read line by line + while (fgets(buffer, MAX_LINE, file) != NULL) { + token = next_token(buffer); + + // empty line found + if (!token) + break; + + line++; + + // verify time value + int temp = str_to_time(token); + if (temp == UNDEFINED_TIME) { + fprintf(stderr, "Error: wbu_motion_new(): expected