From de3f8cb31b613017fc86ae5355d091efc9c223d7 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Tue, 1 Nov 2022 09:33:38 +0100 Subject: [PATCH 01/61] Sync submodule (#501) Co-authored-by: ad-daniel --- webots_ros2_driver/webots | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_driver/webots b/webots_ros2_driver/webots index c8bc1731a..ecc85fe86 160000 --- a/webots_ros2_driver/webots +++ b/webots_ros2_driver/webots @@ -1 +1 @@ -Subproject commit c8bc1731a874e7ed03326e3a0de65f11b6ae3a31 +Subproject commit ecc85fe863419167d7f535d7c5add6417c7133fe From 891294a93b972ccdc7515db83cce119b62f97cb6 Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Thu, 3 Nov 2022 12:01:46 +0100 Subject: [PATCH 02/61] Sync submodule (#507) Co-authored-by: ad-daniel --- webots_ros2_driver/webots | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/webots_ros2_driver/webots b/webots_ros2_driver/webots index ecc85fe86..847bb7af3 160000 --- a/webots_ros2_driver/webots +++ b/webots_ros2_driver/webots @@ -1 +1 @@ -Subproject commit ecc85fe863419167d7f535d7c5add6417c7133fe +Subproject commit 847bb7af32dcc0d4d16d776768d63a7c8bab3db8 From 67e333002c6912142bb0b126f894cad27c2d989c Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Fri, 18 Nov 2022 13:40:32 +0100 Subject: [PATCH 03/61] Merge master into develop (#524) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Sync submodule (#498) Co-authored-by: ad-daniel * Update link to wiki page in README.md (#499) * Update README.md * Update README.md Co-authored-by: Olivier Michel * Keep webots and libController to R2022b rev1 * Update robot launch files (#502) * update changelog and packages.xml (#506) * Fix camera focal length (#510) * Fix camera focal length * Update CHANGELOG.rst * Update CHANGELOG.rst Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Sync submodule (#518) Co-authored-by: ad-daniel * Sync submodule (#521) Co-authored-by: ad-daniel * Upgrade urdf2webots to 2.0.3 (#516) * Upgrade urdf2webots to 2.0.3 * Fix * Fix * Update calculation of `CameraRecognitionObject` (#523) * Update calculation of CameraRecognitionObject Calculation now conforms to the new camera coordinate frame introduced with Webots R2022b. Signed-off-by: Andreas Kreutz * Add test for recognition object calculation Signed-off-by: Andreas Kreutz * Add missing dependency Signed-off-by: Andreas Kreutz * Fix linting error Signed-off-by: Andreas Kreutz * Update changelogs Signed-off-by: Andreas Kreutz Signed-off-by: Andreas Kreutz Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> * Update webots * update changelog and packages.xml (#526) * Keep correct version of Webots Signed-off-by: Andreas Kreutz Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com> Co-authored-by: ad-daniel Co-authored-by: Stefania Pedrazzi Co-authored-by: Olivier Michel Co-authored-by: Darko Lukić Co-authored-by: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Co-authored-by: Andreas Kreutz --- README.md | 2 +- webots_ros2/CHANGELOG.rst | 15 +++++ webots_ros2/package.xml | 2 +- webots_ros2/setup.py | 2 +- webots_ros2_control/package.xml | 2 +- webots_ros2_core/package.xml | 2 +- webots_ros2_core/setup.py | 2 +- webots_ros2_driver/CHANGELOG.rst | 13 ++++ webots_ros2_driver/CMakeLists.txt | 3 + .../plugins/static/Ros2Camera.hpp | 2 + webots_ros2_driver/package.xml | 3 +- .../src/plugins/static/Ros2Camera.cpp | 30 +++++---- webots_ros2_epuck/CHANGELOG.rst | 5 ++ webots_ros2_epuck/launch/robot_launch.py | 59 ++++++++++++----- webots_ros2_epuck/package.xml | 2 +- webots_ros2_epuck/setup.py | 2 +- webots_ros2_importer/CHANGELOG.rst | 4 ++ webots_ros2_importer/package.xml | 2 +- webots_ros2_importer/setup.py | 2 +- .../webots_ros2_importer/urdf2proto.py | 58 ++++++++-------- .../webots_ros2_importer/urdf2webots | 2 +- webots_ros2_mavic/CHANGELOG.rst | 5 ++ webots_ros2_mavic/launch/robot_launch.py | 49 ++++++++++---- webots_ros2_mavic/package.xml | 2 +- webots_ros2_mavic/setup.py | 2 +- webots_ros2_msgs/package.xml | 2 +- webots_ros2_tesla/CHANGELOG.rst | 5 ++ webots_ros2_tesla/launch/robot_launch.py | 51 ++++++++++---- webots_ros2_tesla/package.xml | 2 +- webots_ros2_tesla/setup.py | 2 +- webots_ros2_tests/CHANGELOG.rst | 4 ++ webots_ros2_tests/package.xml | 2 +- webots_ros2_tests/setup.py | 2 +- webots_ros2_tests/test/test_system_driver.py | 11 ++++ webots_ros2_tests/worlds/.driver_test.wbproj | 1 + webots_ros2_tests/worlds/driver_test.wbt | 22 +++++++ webots_ros2_tiago/CHANGELOG.rst | 5 ++ webots_ros2_tiago/launch/robot_launch.py | 66 +++++++++++++------ webots_ros2_tiago/package.xml | 2 +- webots_ros2_tiago/setup.py | 2 +- webots_ros2_turtlebot/CHANGELOG.rst | 5 ++ webots_ros2_turtlebot/launch/robot_launch.py | 58 +++++++++++----- webots_ros2_turtlebot/package.xml | 2 +- webots_ros2_turtlebot/setup.py | 2 +- webots_ros2_universal_robot/CHANGELOG.rst | 4 ++ webots_ros2_universal_robot/package.xml | 2 +- webots_ros2_universal_robot/setup.py | 2 +- 47 files changed, 380 insertions(+), 146 deletions(-) diff --git a/README.md b/README.md index 646cbb839..5c78911c2 100644 --- a/README.md +++ b/README.md @@ -12,7 +12,7 @@ It integrates with ROS2 using ROS2 messages, services and actions. Please visit the [documentation](https://github.com/cyberbotics/webots_ros2/wiki) that contains the following sections: - [Getting Started](https://github.com/cyberbotics/webots_ros2/wiki/Getting-Started) -- [Build and Install](https://github.com/cyberbotics/webots_ros2/wiki/Build-and-Install) +- [Complete Installation Guide](https://github.com/cyberbotics/webots_ros2/wiki/Complete-Installation-Guide) - [Tutorials](https://github.com/cyberbotics/webots_ros2/wiki/Tutorials) - [Examples](https://github.com/cyberbotics/webots_ros2/wiki/Examples) - [References](https://github.com/cyberbotics/webots_ros2/wiki/References) diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 106cd729a..51314a162 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package webots_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.4 (2022-11-18) +------------------ +* Fix the camera focal length in the CameraInfo topic. +* Upgraded to urdf2webots 2.0.3 +* Update the calculation of CameraRecognitionObject messages to the RDF convention of R2022b. + +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to all examples to support simulation reset from Webots. + +2022.1.2 (2022-10-21) +------------------ +* Added WSL support. + 2022.1.0 (2022-09-23) ------------------ * Adapted controllers to communicate with Webots R2022b. diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml index dee6f89d8..bcfdca8a5 100644 --- a/webots_ros2/package.xml +++ b/webots_ros2/package.xml @@ -2,7 +2,7 @@ webots_ros2 - 2022.1.2 + 2022.1.4 Interface between Webots and ROS2 Cyberbotics diff --git a/webots_ros2/setup.py b/webots_ros2/setup.py index c553936bd..bb52ef24f 100644 --- a/webots_ros2/setup.py +++ b/webots_ros2/setup.py @@ -6,7 +6,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=[ ('share/' + package_name, ['package.xml']), diff --git a/webots_ros2_control/package.xml b/webots_ros2_control/package.xml index 3c81ea621..16b97a1fc 100644 --- a/webots_ros2_control/package.xml +++ b/webots_ros2_control/package.xml @@ -2,7 +2,7 @@ webots_ros2_control - 2022.1.2 + 2022.1.4 ros2_control plugin for Webots Cyberbotics http://wiki.ros.org/webots_ros2 diff --git a/webots_ros2_core/package.xml b/webots_ros2_core/package.xml index 2e2cf56a2..81c1efe63 100644 --- a/webots_ros2_core/package.xml +++ b/webots_ros2_core/package.xml @@ -2,7 +2,7 @@ webots_ros2_core - 2022.1.2 + 2022.1.4 Core interface between Webots and ROS2 Cyberbotics diff --git a/webots_ros2_core/setup.py b/webots_ros2_core/setup.py index 820c46ae4..e416d5aa9 100644 --- a/webots_ros2_core/setup.py +++ b/webots_ros2_core/setup.py @@ -19,7 +19,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name, package_name + '.devices', package_name + '.math', package_name + '.webots'], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 18aee5bea..3fdf5bc07 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/webots_ros2_driver/CHANGELOG.rst @@ -2,11 +2,24 @@ Changelog for package webots_ros2_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.4 (2022-11-18) +------------------ +* Fix the camera focal length in the CameraInfo topic. +* Update the calculation of CameraRecognitionObject messages to the RDF convention of R2022b. + +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. + 2022.1.2 (2022-10-21) ------------------ * Fix issue where relatively defined PROTO were not found. * Added WSL support. +2022.1.1 (2022-10-11) +------------------ +* Simplified the detection of Webots installation folder. + 2022.1.0 (2022-09-23) ------------------ * Added an URDF importer feature to spawn robots from URDF files. diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index 82ceb64e3..d84995e52 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/webots_ros2_driver/CMakeLists.txt @@ -32,6 +32,7 @@ find_package(rclcpp REQUIRED) find_package(pluginlib REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(vision_msgs REQUIRED) find_package(webots_ros2_msgs REQUIRED) @@ -137,6 +138,7 @@ ament_target_dependencies(driver pluginlib sensor_msgs std_msgs + tf2_geometry_msgs tf2_ros vision_msgs webots_ros2_msgs @@ -258,6 +260,7 @@ ament_export_dependencies( rclpy sensor_msgs std_msgs + tf2_geometry_msgs tf2_ros vision_msgs webots_ros2_msgs diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp index 59659ea1f..923054c50 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp @@ -21,6 +21,8 @@ #include #include +#include +#include #include #include #include diff --git a/webots_ros2_driver/package.xml b/webots_ros2_driver/package.xml index 1efe137b2..e9ed36786 100644 --- a/webots_ros2_driver/package.xml +++ b/webots_ros2_driver/package.xml @@ -1,7 +1,7 @@ webots_ros2_driver - 2022.1.2 + 2022.1.4 Implementation of the Webots - ROS 2 interface Cyberbotics Apache License 2.0 @@ -21,6 +21,7 @@ sensor_msgs std_msgs geometry_msgs + tf2_geometry_msgs tf2_ros vision_msgs tinyxml2_vendor diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index 2719ec402..ca69dbf2d 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp @@ -44,9 +44,7 @@ namespace webots_ros2_driver mCameraInfoMessage.distortion_model = "plumb_bob"; // Convert FoV to focal length. - // Reference: https://en.wikipedia.org/wiki/Focal_length#In_photography - const double diagonal = sqrt(pow(mCamera->getWidth(), 2) + pow(mCamera->getHeight(), 2)); - const double focalLength = 0.5 * diagonal * (cos(0.5 * mCamera->getFov()) / sin(0.5 * mCamera->getFov())); + const double focalLength = mCamera->getWidth() / (2 * tan(mCamera->getFov() / 2)); mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; @@ -138,18 +136,25 @@ namespace webots_ros2_driver for (size_t i = 0; i < mCamera->getRecognitionNumberOfObjects(); i++) { // Getting Object Info - geometry_msgs::msg::Point position; - geometry_msgs::msg::Quaternion orientation; - position.x = objects[i].position[0]; - position.y = objects[i].position[1]; - position.z = objects[i].position[2]; - axisAngleToQuaternion(objects[i].orientation, orientation); + geometry_msgs::msg::PoseStamped pose; + pose.pose.position.x = objects[i].position[0]; + pose.pose.position.y = objects[i].position[1]; + pose.pose.position.z = objects[i].position[2]; + axisAngleToQuaternion(objects[i].orientation, pose.pose.orientation); + + // Transform to ROS camera coordinate frame + // rpy = (0, pi/2, -pi/2) + geometry_msgs::msg::TransformStamped transform; + transform.transform.rotation.x = 0.5; + transform.transform.rotation.y = -0.5; + transform.transform.rotation.z = 0.5; + transform.transform.rotation.w = 0.5; + tf2::doTransform(pose, pose, transform); // Object Info -> Detection2D vision_msgs::msg::Detection2D detection; vision_msgs::msg::ObjectHypothesisWithPose hypothesis; - hypothesis.pose.pose.position = position; - hypothesis.pose.pose.orientation = orientation; + hypothesis.pose.pose = pose.pose; detection.results.push_back(hypothesis); #if FOXY || GALACTIC detection.bbox.center.x = objects[i].position_on_image[0]; @@ -166,8 +171,7 @@ namespace webots_ros2_driver webots_ros2_msgs::msg::CameraRecognitionObject recognitionWebotsObject; recognitionWebotsObject.id = objects[i].id; recognitionWebotsObject.model = std::string(objects[i].model); - recognitionWebotsObject.pose.pose.position = position; - recognitionWebotsObject.pose.pose.orientation = orientation; + recognitionWebotsObject.pose = pose; #if FOXY || GALACTIC recognitionWebotsObject.bbox.center.x = objects[i].position_on_image[0]; recognitionWebotsObject.bbox.center.y = objects[i].position_on_image[1]; diff --git a/webots_ros2_epuck/CHANGELOG.rst b/webots_ros2_epuck/CHANGELOG.rst index 5c1fb9fb9..d9a06edd1 100644 --- a/webots_ros2_epuck/CHANGELOG.rst +++ b/webots_ros2_epuck/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_epuck ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to support simulation reset from Webots. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_epuck/launch/robot_launch.py b/webots_ros2_epuck/launch/robot_launch.py index ce194cfc5..7d1e0181f 100644 --- a/webots_ros2_epuck/launch/robot_launch.py +++ b/webots_ros2_epuck/launch/robot_launch.py @@ -29,19 +29,11 @@ from webots_ros2_driver.utils import controller_url_prefix -def generate_launch_description(): +def get_ros2_nodes(*args): package_dir = get_package_share_directory('webots_ros2_epuck') - world = LaunchConfiguration('world') robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'epuck_webots.urdf')).read_text() ros2_control_params = os.path.join(package_dir, 'resource', 'ros2_control.yml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]) - ) - - ros2_supervisor = Ros2SupervisorLauncher() - controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -112,6 +104,35 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], ) + return [ + joint_state_broadcaster_spawner, + diffdrive_controller_spawner, + robot_state_publisher, + epuck_driver, + footprint_publisher, + epuck_process, + ] + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_epuck') + world = LaunchConfiguration('world') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]) + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + # The following line is important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ros2_supervisor, + on_exit=get_ros2_nodes, + ) + ) + return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -120,18 +141,20 @@ def generate_launch_description(): ), webots, ros2_supervisor, - joint_state_broadcaster_spawner, - diffdrive_controller_spawner, - robot_state_publisher, - epuck_driver, - footprint_publisher, - epuck_process, # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], ) - ) - ]) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_epuck/package.xml b/webots_ros2_epuck/package.xml index 0da64901f..7dc27958c 100644 --- a/webots_ros2_epuck/package.xml +++ b/webots_ros2_epuck/package.xml @@ -2,7 +2,7 @@ webots_ros2_epuck - 2022.1.2 + 2022.1.4 E-puck2 driver for Webots simulated robot Cyberbotics diff --git a/webots_ros2_epuck/setup.py b/webots_ros2_epuck/setup.py index b639f3916..fab280105 100644 --- a/webots_ros2_epuck/setup.py +++ b/webots_ros2_epuck/setup.py @@ -43,7 +43,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_importer/CHANGELOG.rst b/webots_ros2_importer/CHANGELOG.rst index a7c06bed7..b0a18601e 100644 --- a/webots_ros2_importer/CHANGELOG.rst +++ b/webots_ros2_importer/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_importer ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.4 (2022-11-18) +------------------ +* Upgraded to urdf2webots 2.0.3 + 2022.1.1 (2022-10-11) ------------------ * Upgraded to urdf2webots 2.0.2 diff --git a/webots_ros2_importer/package.xml b/webots_ros2_importer/package.xml index 96d3adc9c..6a9da58d7 100644 --- a/webots_ros2_importer/package.xml +++ b/webots_ros2_importer/package.xml @@ -2,7 +2,7 @@ webots_ros2_importer - 2022.1.2 + 2022.1.4 This package allows to convert URDF and XACRO files into Webots PROTO files. Cyberbotics diff --git a/webots_ros2_importer/setup.py b/webots_ros2_importer/setup.py index 22207e6a7..b9423251b 100644 --- a/webots_ros2_importer/setup.py +++ b/webots_ros2_importer/setup.py @@ -10,7 +10,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name, package_name + '.urdf2webots.urdf2webots'], data_files=data_files, install_requires=[ diff --git a/webots_ros2_importer/webots_ros2_importer/urdf2proto.py b/webots_ros2_importer/webots_ros2_importer/urdf2proto.py index 181cb952e..f5ad5caf4 100644 --- a/webots_ros2_importer/webots_ros2_importer/urdf2proto.py +++ b/webots_ros2_importer/webots_ros2_importer/urdf2proto.py @@ -25,41 +25,41 @@ def main(args=None, urdfInput=None): parser = argparse.ArgumentParser(usage='usage: %prog --input=my_robot.urdf [options]') - parser.add_option('--input', dest='input', default='', help='Specifies the URDF file.') - parser.add_option('--output', dest='output', default='', help='Specifies the path and, if ending in ".proto", name ' - 'of the resulting PROTO file. The filename minus the .proto extension will be the robot name ' - '(for PROTO conversion only).') - parser.add_option('--normal', dest='normal', action='store_true', default=False, - help='If set, the normals are exported if present in the URDF definition.') - parser.add_option('--box-collision', dest='boxCollision', action='store_true', default=False, - help='If set, the bounding objects are approximated using boxes.') - parser.add_option('--tool-slot', dest='toolSlot', default=None, - help='Specify the link that you want to add a tool slot too (exact link name from URDF, for PROTO ' - 'conversion only).') - parser.add_option('--translation', dest='initTranslation', default='0 0 0', - help='Set the translation field of your PROTO file or Webots VRML robot string.') - parser.add_option('--rotation', dest='initRotation', default='0 0 1 0', - help='Set the rotation field of your PROTO file or Webots Robot node string.') - parser.add_option('--init-pos', dest='initPos', default=None, - help='Set the initial positions of your robot joints. Example: --init-pos="[1.2, 0.5, -1.5]" would ' - 'set the first 3 joints of your robot to the specified values, and leave the rest with their ' - 'default value.') - parser.add_option('--link-to-def', dest='linkToDef', action='store_true', default=False, - help='Creates a DEF with the link name for each solid to be able to access it using ' - 'getFromProtoDef(defName) (for PROTO conversion only).') - parser.add_option('--joint-to-def', dest='jointToDef', action='store_true', default=False, - help='Creates a DEF with the joint name for each joint to be able to access it using ' - 'getFromProtoDef(defName) (for PROTO conversion only).') + parser.add_argument('--input', dest='input', default='', help='Specifies the URDF file.') + parser.add_argument('--output', dest='output', default='', help='Specifies the path and, if ending in ".proto", name ' + 'of the resulting PROTO file. The filename minus the .proto extension will be the robot name ' + '(for PROTO conversion only).') + parser.add_argument('--normal', dest='normal', action='store_true', default=False, + help='If set, the normals are exported if present in the URDF definition.') + parser.add_argument('--box-collision', dest='boxCollision', action='store_true', default=False, + help='If set, the bounding objects are approximated using boxes.') + parser.add_argument('--tool-slot', dest='toolSlot', default=None, + help='Specify the link that you want to add a tool slot too (exact link name from URDF, for PROTO ' + 'conversion only).') + parser.add_argument('--translation', dest='initTranslation', default='0 0 0', + help='Set the translation field of your PROTO file or Webots VRML robot string.') + parser.add_argument('--rotation', dest='initRotation', default='0 0 1 0', + help='Set the rotation field of your PROTO file or Webots Robot node string.') + parser.add_argument('--init-pos', dest='initPos', default=None, + help='Set the initial positions of your robot joints. Example: --init-pos="[1.2, 0.5, -1.5]" would ' + 'set the first 3 joints of your robot to the specified values, and leave the rest with their ' + 'default value.') + parser.add_argument('--link-to-def', dest='linkToDef', action='store_true', default=False, + help='Creates a DEF with the link name for each solid to be able to access it using ' + 'getFromProtoDef(defName) (for PROTO conversion only).') + parser.add_argument('--joint-to-def', dest='jointToDef', action='store_true', default=False, + help='Creates a DEF with the joint name for each joint to be able to access it using ' + 'getFromProtoDef(defName) (for PROTO conversion only).') # use 'parse_known_args' because ROS2 adds a lot of internal arguments arguments, _ = parser.parse_known_args() - file = os.path.abspath(urdfInput) if urdfInput is not None else os.path.abspath(arguments.inputConversion) - if not file: + file = os.path.abspath(urdfInput) if urdfInput is not None else os.path.abspath(arguments.input) + if not arguments.input: sys.exit('Input file not specified (should be specified with the "--input" argument).') if not os.path.isfile(file): - sys.exit('"%s" file does not exist.' % arguments.inputConversion) + sys.exit('"%s" file does not exist.' % arguments.input) elif not file.endswith('.urdf'): sys.exit('"%s" is not an urdf file.' % file) - convertUrdfFile(input=arguments.inputConversion, + convertUrdfFile(input=arguments.input, output=arguments.output, normal=arguments.normal, boxCollision=arguments.boxCollision, diff --git a/webots_ros2_importer/webots_ros2_importer/urdf2webots b/webots_ros2_importer/webots_ros2_importer/urdf2webots index 23bf5992b..4dc4613a2 160000 --- a/webots_ros2_importer/webots_ros2_importer/urdf2webots +++ b/webots_ros2_importer/webots_ros2_importer/urdf2webots @@ -1 +1 @@ -Subproject commit 23bf5992bb9698bcc68f15fc2e56c9def938bde6 +Subproject commit 4dc4613a276995d211b0377a52c4a23068f3a890 diff --git a/webots_ros2_mavic/CHANGELOG.rst b/webots_ros2_mavic/CHANGELOG.rst index 6a063f565..920a7e0a0 100644 --- a/webots_ros2_mavic/CHANGELOG.rst +++ b/webots_ros2_mavic/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_mavic ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to support simulation reset from Webots. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_mavic/launch/robot_launch.py b/webots_ros2_mavic/launch/robot_launch.py index 5de55f7ef..1ddb71f3c 100644 --- a/webots_ros2_mavic/launch/robot_launch.py +++ b/webots_ros2_mavic/launch/robot_launch.py @@ -29,17 +29,10 @@ from webots_ros2_driver.utils import controller_url_prefix -def generate_launch_description(): +def get_ros2_nodes(*args): package_dir = get_package_share_directory('webots_ros2_mavic') - world = LaunchConfiguration('world') robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'mavic_webots.urdf')).read_text() - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]) - ) - - ros2_supervisor = Ros2SupervisorLauncher() - mavic_driver = Node( package='webots_ros2_driver', executable='driver', @@ -50,6 +43,30 @@ def generate_launch_description(): ] ) + return [ + mavic_driver, + ] + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_mavic') + world = LaunchConfiguration('world') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]) + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + # The following line is important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ros2_supervisor, + on_exit=get_ros2_nodes, + ) + ) + return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -58,12 +75,20 @@ def generate_launch_description(): ), webots, ros2_supervisor, - mavic_driver, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], ) - ) - ]) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_mavic/package.xml b/webots_ros2_mavic/package.xml index 994d88f34..90de6a117 100644 --- a/webots_ros2_mavic/package.xml +++ b/webots_ros2_mavic/package.xml @@ -2,7 +2,7 @@ webots_ros2_mavic - 2022.1.2 + 2022.1.4 Mavic 2 Pro robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_mavic/setup.py b/webots_ros2_mavic/setup.py index 4c63eb9b6..9b515c62e 100644 --- a/webots_ros2_mavic/setup.py +++ b/webots_ros2_mavic/setup.py @@ -18,7 +18,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_msgs/package.xml b/webots_ros2_msgs/package.xml index 78ae625b4..5c23aaa26 100644 --- a/webots_ros2_msgs/package.xml +++ b/webots_ros2_msgs/package.xml @@ -2,7 +2,7 @@ webots_ros2_msgs - 2022.1.2 + 2022.1.4 Services and Messages of the webots_ros2 packages. Cyberbotics diff --git a/webots_ros2_tesla/CHANGELOG.rst b/webots_ros2_tesla/CHANGELOG.rst index c6e97591f..f4c025541 100644 --- a/webots_ros2_tesla/CHANGELOG.rst +++ b/webots_ros2_tesla/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_tesla ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to support simulation reset from Webots. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_tesla/launch/robot_launch.py b/webots_ros2_tesla/launch/robot_launch.py index a3a8db32d..5e2b95ede 100644 --- a/webots_ros2_tesla/launch/robot_launch.py +++ b/webots_ros2_tesla/launch/robot_launch.py @@ -29,17 +29,10 @@ from webots_ros2_driver.utils import controller_url_prefix -def generate_launch_description(): +def get_ros2_nodes(*args): package_dir = get_package_share_directory('webots_ros2_tesla') - world = LaunchConfiguration('world') robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'tesla_webots.urdf')).read_text() - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]) - ) - - ros2_supervisor = Ros2SupervisorLauncher() - tesla_driver = Node( package='webots_ros2_driver', executable='driver', @@ -55,6 +48,31 @@ def generate_launch_description(): executable='lane_follower', ) + return [ + lane_follower, + tesla_driver, + ] + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_tesla') + world = LaunchConfiguration('world') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]) + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + # The following line is important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ros2_supervisor, + on_exit=get_ros2_nodes, + ) + ) + return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -63,13 +81,20 @@ def generate_launch_description(): ), webots, ros2_supervisor, - lane_follower, - tesla_driver, + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], ) - ) - ]) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_tesla/package.xml b/webots_ros2_tesla/package.xml index 85c14251d..73d0ce1bf 100644 --- a/webots_ros2_tesla/package.xml +++ b/webots_ros2_tesla/package.xml @@ -2,7 +2,7 @@ webots_ros2_tesla - 2022.1.2 + 2022.1.4 Tesla ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_tesla/setup.py b/webots_ros2_tesla/setup.py index 52498e752..63f555894 100644 --- a/webots_ros2_tesla/setup.py +++ b/webots_ros2_tesla/setup.py @@ -18,7 +18,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_tests/CHANGELOG.rst b/webots_ros2_tests/CHANGELOG.rst index 602db674e..dccdcf3ab 100644 --- a/webots_ros2_tests/CHANGELOG.rst +++ b/webots_ros2_tests/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.4 (2022-11-18) +------------------ +* Add a system test for the R2022b RDF convention for cameras. + 1.2.1 (2022-01-10) ------------------ * Add a system test for the FLU lidar device. diff --git a/webots_ros2_tests/package.xml b/webots_ros2_tests/package.xml index 30373e25a..dd65cf2a8 100644 --- a/webots_ros2_tests/package.xml +++ b/webots_ros2_tests/package.xml @@ -2,7 +2,7 @@ webots_ros2_tests - 2022.1.2 + 2022.1.4 System tests for `webots_ros2` packages. Cyberbotics diff --git a/webots_ros2_tests/setup.py b/webots_ros2_tests/setup.py index d759405e4..1781f6ad9 100644 --- a/webots_ros2_tests/setup.py +++ b/webots_ros2_tests/setup.py @@ -11,7 +11,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_tests/test/test_system_driver.py b/webots_ros2_tests/test/test_system_driver.py index d49fc6270..1d59cd417 100644 --- a/webots_ros2_tests/test/test_system_driver.py +++ b/webots_ros2_tests/test/test_system_driver.py @@ -28,6 +28,7 @@ from sensor_msgs.msg import Range, Image, Imu, Illuminance from std_msgs.msg import Int32, Float32 from geometry_msgs.msg import PointStamped, Vector3 +from webots_ros2_msgs.msg import CameraRecognitionObjects from launch import LaunchDescription from launch_ros.actions import Node import launch @@ -124,6 +125,16 @@ def on_image_received(message): self.wait_for_messages(self.__node, Image, '/Pioneer_3_AT/kinect_color', condition=on_image_received) + def testRecognition(self): + def on_objects_received(message): + self.assertAlmostEqual(message.objects[0].pose.pose.position.x, 0.08, delta=0.01) + self.assertAlmostEqual(message.objects[0].pose.pose.position.y, -0.09, delta=0.01) + self.assertAlmostEqual(message.objects[0].pose.pose.position.z, 0.55, delta=0.01) + return True + + self.wait_for_messages(self.__node, CameraRecognitionObjects, '/Pioneer_3_AT/camera/recognitions/webots', + condition=on_objects_received) + def testPythonPluginService(self): client = self.__node.create_client(Trigger, 'move_forward') if not client.wait_for_service(timeout_sec=10.0): diff --git a/webots_ros2_tests/worlds/.driver_test.wbproj b/webots_ros2_tests/worlds/.driver_test.wbproj index f5258d26b..3a77c2275 100644 --- a/webots_ros2_tests/worlds/.driver_test.wbproj +++ b/webots_ros2_tests/worlds/.driver_test.wbproj @@ -9,3 +9,4 @@ textFiles: -1 consoles: Console:All:All renderingDevicePerspectives: Pioneer 3-AT:kinect color;1;1;0;0 renderingDevicePerspectives: Pioneer 3-AT:kinect range;1;1;0;0 +renderingDevicePerspectives: Pioneer 3-AT:camera;0;1;0;0 diff --git a/webots_ros2_tests/worlds/driver_test.wbt b/webots_ros2_tests/worlds/driver_test.wbt index 4f1a135ef..ef989a850 100644 --- a/webots_ros2_tests/worlds/driver_test.wbt +++ b/webots_ros2_tests/worlds/driver_test.wbt @@ -8,6 +8,7 @@ EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/project EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/robotis/protos/RobotisLds01.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/devices/microsoft/protos/Kinect.proto" EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/Plaster.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2022b/projects/appearances/protos/RedBricks.proto" WorldInfo { } @@ -25,6 +26,21 @@ CardboardBox { translation 1.21847 0.00917563 0.3 rotation 0.7040631892342535 -0.09269012491274485 -0.7040621892339847 -2.9567353071795863 } +Solid { + translation -0.5 0.08 0.34 + children [ + Shape { + appearance RedBricks { + } + geometry Box { + size 0.1 0.1 0.1 + } + } + ] + recognitionColors [ + 1 0 0 + ] +} Pioneer3at { name "Pioneer_3_AT" controller "" @@ -66,5 +82,11 @@ Pioneer3at { physics Physics { } } + Camera { + translation -0.25 0 0.25 + rotation 0 0 1 3.14159 + recognition Recognition { + } + } ] } diff --git a/webots_ros2_tiago/CHANGELOG.rst b/webots_ros2_tiago/CHANGELOG.rst index dd9e84560..6f23f315d 100644 --- a/webots_ros2_tiago/CHANGELOG.rst +++ b/webots_ros2_tiago/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_tiago ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to support simulation reset from Webots. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_tiago/launch/robot_launch.py b/webots_ros2_tiago/launch/robot_launch.py index e6b924109..d9533f6a5 100644 --- a/webots_ros2_tiago/launch/robot_launch.py +++ b/webots_ros2_tiago/launch/robot_launch.py @@ -31,11 +31,9 @@ from webots_ros2_driver.utils import controller_url_prefix -def generate_launch_description(): +def get_ros2_nodes(*args): optional_nodes = [] package_dir = get_package_share_directory('webots_ros2_tiago') - world = LaunchConfiguration('world') - mode = LaunchConfiguration('mode') use_rviz = LaunchConfiguration('rviz', default=False) use_nav = LaunchConfiguration('nav', default=False) use_slam = LaunchConfiguration('slam', default=False) @@ -44,13 +42,6 @@ def generate_launch_description(): nav2_map = os.path.join(package_dir, 'resource', 'map.yaml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]), - mode=mode - ) - - ros2_supervisor = Ros2SupervisorLauncher() - controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -135,6 +126,38 @@ def generate_launch_description(): condition=launch.conditions.IfCondition(use_slam) ) + return [ + joint_state_broadcaster_spawner, + diffdrive_controller_spawner, + rviz, + robot_state_publisher, + tiago_driver, + footprint_publisher, + slam_toolbox, + ] + optional_nodes + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_tiago') + world = LaunchConfiguration('world') + mode = LaunchConfiguration('mode') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]), + mode=mode + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + # The following line is important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ros2_supervisor, + on_exit=get_ros2_nodes, + ) + ) + return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -148,17 +171,20 @@ def generate_launch_description(): ), webots, ros2_supervisor, - joint_state_broadcaster_spawner, - diffdrive_controller_spawner, - rviz, - robot_state_publisher, - tiago_driver, - footprint_publisher, - slam_toolbox, + + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], ) - ) - ] + optional_nodes) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_tiago/package.xml b/webots_ros2_tiago/package.xml index 34ae61e9d..1a5d883ce 100644 --- a/webots_ros2_tiago/package.xml +++ b/webots_ros2_tiago/package.xml @@ -2,7 +2,7 @@ webots_ros2_tiago - 2022.1.2 + 2022.1.4 TIAGo robots ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_tiago/setup.py b/webots_ros2_tiago/setup.py index acf3bb8e5..a57e2d285 100644 --- a/webots_ros2_tiago/setup.py +++ b/webots_ros2_tiago/setup.py @@ -17,7 +17,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_turtlebot/CHANGELOG.rst b/webots_ros2_turtlebot/CHANGELOG.rst index 604b437ff..02eb1be29 100644 --- a/webots_ros2_turtlebot/CHANGELOG.rst +++ b/webots_ros2_turtlebot/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package webots_ros2_turtlebot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. +* Added reset handler to support simulation reset from Webots. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_turtlebot/launch/robot_launch.py b/webots_ros2_turtlebot/launch/robot_launch.py index 28a710a2a..587e7904a 100644 --- a/webots_ros2_turtlebot/launch/robot_launch.py +++ b/webots_ros2_turtlebot/launch/robot_launch.py @@ -29,19 +29,12 @@ from webots_ros2_driver.utils import controller_url_prefix -def generate_launch_description(): +def get_ros2_nodes(*args): package_dir = get_package_share_directory('webots_ros2_turtlebot') - world = LaunchConfiguration('world') robot_description = pathlib.Path(os.path.join(package_dir, 'resource', 'turtlebot_webots.urdf')).read_text() ros2_control_params = os.path.join(package_dir, 'resource', 'ros2control.yml') use_sim_time = LaunchConfiguration('use_sim_time', default=True) - webots = WebotsLauncher( - world=PathJoinSubstitution([package_dir, 'worlds', world]) - ) - - ros2_supervisor = Ros2SupervisorLauncher() - # TODO: Revert once the https://github.com/ros-controls/ros2_control/pull/444 PR gets into the release controller_manager_timeout = ['--controller-manager-timeout', '50'] controller_manager_prefix = 'python.exe' if os.name == 'nt' else '' @@ -98,6 +91,34 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_footprint'], ) + return [ + joint_state_broadcaster_spawner, + diffdrive_controller_spawner, + robot_state_publisher, + turtlebot_driver, + footprint_publisher, + ] + + +def generate_launch_description(): + package_dir = get_package_share_directory('webots_ros2_turtlebot') + world = LaunchConfiguration('world') + + webots = WebotsLauncher( + world=PathJoinSubstitution([package_dir, 'worlds', world]) + ) + + ros2_supervisor = Ros2SupervisorLauncher() + + # The following line is important! + # This event handler respawns the ROS 2 nodes on simulation reset (supervisor process ends). + reset_handler = launch.actions.RegisterEventHandler( + event_handler=launch.event_handlers.OnProcessExit( + target_action=ros2_supervisor, + on_exit=get_ros2_nodes, + ) + ) + return LaunchDescription([ DeclareLaunchArgument( 'world', @@ -106,15 +127,20 @@ def generate_launch_description(): ), webots, ros2_supervisor, - joint_state_broadcaster_spawner, - diffdrive_controller_spawner, - robot_state_publisher, - turtlebot_driver, - footprint_publisher, + + # This action will kill all nodes once the Webots simulation has exited launch.actions.RegisterEventHandler( event_handler=launch.event_handlers.OnProcessExit( target_action=webots, - on_exit=[launch.actions.EmitEvent(event=launch.events.Shutdown())], + on_exit=[ + launch.actions.UnregisterEventHandler( + event_handler=reset_handler.event_handler + ), + launch.actions.EmitEvent(event=launch.events.Shutdown()) + ], ) - ) - ]) + ), + + # Add the reset event handler + reset_handler + ] + get_ros2_nodes()) diff --git a/webots_ros2_turtlebot/package.xml b/webots_ros2_turtlebot/package.xml index d29826dee..5213c2b38 100644 --- a/webots_ros2_turtlebot/package.xml +++ b/webots_ros2_turtlebot/package.xml @@ -2,7 +2,7 @@ webots_ros2_turtlebot - 2022.1.2 + 2022.1.4 TurtleBot3 Burger robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_turtlebot/setup.py b/webots_ros2_turtlebot/setup.py index d77d5ad89..cf43842d0 100644 --- a/webots_ros2_turtlebot/setup.py +++ b/webots_ros2_turtlebot/setup.py @@ -22,7 +22,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=[package_name], data_files=data_files, install_requires=['setuptools', 'launch'], diff --git a/webots_ros2_universal_robot/CHANGELOG.rst b/webots_ros2_universal_robot/CHANGELOG.rst index d49cfb990..e88a84db2 100644 --- a/webots_ros2_universal_robot/CHANGELOG.rst +++ b/webots_ros2_universal_robot/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_universal_robot ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2022.1.3 (2022-11-02) +------------------ +* Added macOS support. + 2022.1.2 (2022-10-21) ------------------ * Added WSL support. diff --git a/webots_ros2_universal_robot/package.xml b/webots_ros2_universal_robot/package.xml index 1b625d81c..d1d0fb2e8 100644 --- a/webots_ros2_universal_robot/package.xml +++ b/webots_ros2_universal_robot/package.xml @@ -2,7 +2,7 @@ webots_ros2_universal_robot - 2022.1.2 + 2022.1.4 Universal Robot ROS2 interface for Webots. Cyberbotics diff --git a/webots_ros2_universal_robot/setup.py b/webots_ros2_universal_robot/setup.py index f70333219..af1c6a40a 100644 --- a/webots_ros2_universal_robot/setup.py +++ b/webots_ros2_universal_robot/setup.py @@ -30,7 +30,7 @@ setup( name=package_name, - version='2022.1.2', + version='2022.1.4', packages=['webots_ros2_universal_robot'], data_files=data_files, install_requires=['setuptools', 'launch'], From 0e45db3ba579de1256cdda5fdec469a293319046 Mon Sep 17 00:00:00 2001 From: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Date: Fri, 18 Nov 2022 14:27:07 +0100 Subject: [PATCH 04/61] Remove deprecared `webots_ros2_core` package (#525) * Remove * Minor * Fix changelog * Fix submodule --- README.md | 14 +- docs/.gitignore | 1 - docs/Makefile | 20 - docs/source/API-Devices.rst | 65 -- docs/source/API-JointStatePublisher.rst | 5 - docs/source/API-Utils.rst | 4 - docs/source/API-WebotsLauncher.rst | 5 - docs/source/API-WebotsNode.rst | 15 - docs/source/_API-Header.rst | 3 - docs/source/_API-Index.rst | 11 - docs/source/conf.py | 94 --- {docs => images}/cover.png | Bin webots_ros2/CHANGELOG.rst | 6 +- webots_ros2/package.xml | 1 - webots_ros2_core/CHANGELOG.rst | 48 -- webots_ros2_core/README.md | 12 - webots_ros2_core/launch/robot_launch.py | 148 ----- webots_ros2_core/package.xml | 32 - webots_ros2_core/resource/webots_ros2_core | 0 webots_ros2_core/setup.cfg | 4 - webots_ros2_core/setup.py | 49 -- webots_ros2_core/test/test_copyright.py | 25 - webots_ros2_core/test/test_flake8.py | 27 - webots_ros2_core/test/test_math_utils.py | 63 -- webots_ros2_core/test/test_pep257.py | 25 - webots_ros2_core/webots_ros2_core/.gitignore | 1 - webots_ros2_core/webots_ros2_core/__init__.py | 20 - .../webots_ros2_core/devices/__init__.py | 13 - .../webots_ros2_core/devices/camera_device.py | 232 ------- .../webots_ros2_core/devices/device.py | 34 - .../devices/device_manager.py | 144 ----- .../devices/distance_sensor_device.py | 95 --- .../webots_ros2_core/devices/gps_device.py | 109 ---- .../webots_ros2_core/devices/imu_device.py | 101 --- .../webots_ros2_core/devices/led_device.py | 67 -- .../webots_ros2_core/devices/lidar_device.py | 138 ----- .../devices/light_sensor_device.py | 94 --- .../devices/range_finder_device.py | 95 --- .../webots_ros2_core/devices/robot_device.py | 94 --- .../webots_ros2_core/devices/sensor_device.py | 67 -- .../webots_ros2_core/joint_state_publisher.py | 78 --- .../webots_ros2_core/math/README.md | 1 - .../webots_ros2_core/math/__init__.py | 13 - .../webots_ros2_core/math/interpolation.py | 63 -- .../webots_ros2_core/math/quaternions.py | 585 ------------------ .../webots_ros2_core/trajectory_follower.py | 220 ------- webots_ros2_core/webots_ros2_core/utils.py | 310 ---------- .../webots_ros2_core/webots/__init__.py | 13 - .../webots_ros2_core/webots/controller.py | 27 - .../webots_ros2_core/webots/vehicle.py | 27 - .../webots_differential_drive_node.py | 234 ------- .../webots_ros2_core/webots_launcher.py | 78 --- .../webots_ros2_core/webots_node.py | 142 ----- .../webots_robotic_arm_node.py | 64 -- webots_ros2_driver/README.md | 4 +- 55 files changed, 13 insertions(+), 3827 deletions(-) delete mode 100644 docs/.gitignore delete mode 100644 docs/Makefile delete mode 100644 docs/source/API-Devices.rst delete mode 100644 docs/source/API-JointStatePublisher.rst delete mode 100644 docs/source/API-Utils.rst delete mode 100644 docs/source/API-WebotsLauncher.rst delete mode 100644 docs/source/API-WebotsNode.rst delete mode 100644 docs/source/_API-Header.rst delete mode 100644 docs/source/_API-Index.rst delete mode 100644 docs/source/conf.py rename {docs => images}/cover.png (100%) delete mode 100644 webots_ros2_core/CHANGELOG.rst delete mode 100644 webots_ros2_core/README.md delete mode 100644 webots_ros2_core/launch/robot_launch.py delete mode 100644 webots_ros2_core/package.xml delete mode 100644 webots_ros2_core/resource/webots_ros2_core delete mode 100644 webots_ros2_core/setup.cfg delete mode 100644 webots_ros2_core/setup.py delete mode 100644 webots_ros2_core/test/test_copyright.py delete mode 100644 webots_ros2_core/test/test_flake8.py delete mode 100644 webots_ros2_core/test/test_math_utils.py delete mode 100644 webots_ros2_core/test/test_pep257.py delete mode 100644 webots_ros2_core/webots_ros2_core/.gitignore delete mode 100644 webots_ros2_core/webots_ros2_core/__init__.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/__init__.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/camera_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/device_manager.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/gps_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/imu_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/led_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/lidar_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/light_sensor_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/range_finder_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/robot_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/devices/sensor_device.py delete mode 100644 webots_ros2_core/webots_ros2_core/joint_state_publisher.py delete mode 100644 webots_ros2_core/webots_ros2_core/math/README.md delete mode 100644 webots_ros2_core/webots_ros2_core/math/__init__.py delete mode 100644 webots_ros2_core/webots_ros2_core/math/interpolation.py delete mode 100644 webots_ros2_core/webots_ros2_core/math/quaternions.py delete mode 100644 webots_ros2_core/webots_ros2_core/trajectory_follower.py delete mode 100644 webots_ros2_core/webots_ros2_core/utils.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots/__init__.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots/controller.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots/vehicle.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots_differential_drive_node.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots_launcher.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots_node.py delete mode 100644 webots_ros2_core/webots_ros2_core/webots_robotic_arm_node.py diff --git a/README.md b/README.md index 5c78911c2..7ec2dedd1 100644 --- a/README.md +++ b/README.md @@ -8,7 +8,7 @@ `webots_ros2` is a package that provides the necessary interfaces to simulate a robot in the [Webots](https://cyberbotics.com/) open-source 3D robots simulator. It integrates with ROS2 using ROS2 messages, services and actions. -![Webots](docs/cover.png) +![Webots](images/cover.png) Please visit the [documentation](https://github.com/cyberbotics/webots_ros2/wiki) that contains the following sections: - [Getting Started](https://github.com/cyberbotics/webots_ros2/wiki/Getting-Started) @@ -25,13 +25,13 @@ Please visit the [documentation](https://github.com/cyberbotics/webots_ros2/wiki alt="rosin_logo" height="60" >
-Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. +Supported by ROSIN - ROS-Industrial Quality-Assured Robot Software Components. More information: rosin-project.eu eu_flag + alt="eu_flag" height="45" align="left" > -This project has received funding from the European Union’s Horizon 2020 +This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement no. 732287.
@@ -41,11 +41,11 @@ research and innovation programme under grant agreement no. 732287. alt="opendr_logo" height="60" >
-Supported by OpenDR - Open Deep Learning Toolkit for Robotics. +Supported by OpenDR - Open Deep Learning Toolkit for Robotics. More information: opendr.eu eu_flag + alt="eu_flag" height="45" align="left" > -This project has received funding from the European Union’s Horizon 2020 +This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement no. 871449. diff --git a/docs/.gitignore b/docs/.gitignore deleted file mode 100644 index 378eac25d..000000000 --- a/docs/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build diff --git a/docs/Makefile b/docs/Makefile deleted file mode 100644 index d0c3cbf10..000000000 --- a/docs/Makefile +++ /dev/null @@ -1,20 +0,0 @@ -# Minimal makefile for Sphinx documentation -# - -# You can set these variables from the command line, and also -# from the environment for the first two. -SPHINXOPTS ?= -SPHINXBUILD ?= sphinx-build -SOURCEDIR = source -BUILDDIR = build - -# Put it first so that "make" without argument is like "make help". -help: - @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) - -.PHONY: help Makefile - -# Catch-all target: route all unknown targets to Sphinx using the new -# "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). -%: Makefile - @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) diff --git a/docs/source/API-Devices.rst b/docs/source/API-Devices.rst deleted file mode 100644 index dd10a2af4..000000000 --- a/docs/source/API-Devices.rst +++ /dev/null @@ -1,65 +0,0 @@ -.. include:: _API-Header.rst - -We created a collection of Python modules that read a Webots node description and create a suitable ROS2 interface. - -## Robot Device - -.. automodule:: webots_ros2_core.devices.robot_device - :members: - :show-inheritance: - - -## LED Device - -.. automodule:: webots_ros2_core.devices.led_device - :members: - :show-inheritance: - - -## SensorDevice - -.. automodule:: webots_ros2_core.devices.sensor_device - :members: - :show-inheritance: - - -### Lidar Device - -.. automodule:: webots_ros2_core.devices.lidar_device - :members: - :show-inheritance: - - -### Camera Device - -.. automodule:: webots_ros2_core.devices.camera_device - :members: - :show-inheritance: - - -### RangeFinder Device - -.. automodule:: webots_ros2_core.devices.range_finder_device - :members: - :show-inheritance: - - -### Distance Sensor Device - -.. automodule:: webots_ros2_core.devices.distance_sensor_device - :members: - :show-inheritance: - - -### IMU Device - -.. automodule:: webots_ros2_core.devices.imu_device - :members: - :show-inheritance: - - -### Light Sensor Device - -.. automodule:: webots_ros2_core.devices.light_sensor_device - :members: - :show-inheritance: diff --git a/docs/source/API-JointStatePublisher.rst b/docs/source/API-JointStatePublisher.rst deleted file mode 100644 index c4c173e23..000000000 --- a/docs/source/API-JointStatePublisher.rst +++ /dev/null @@ -1,5 +0,0 @@ -.. include:: _API-Header.rst - -.. automodule:: webots_ros2_core.joint_state_publisher - :members: - :show-inheritance: diff --git a/docs/source/API-Utils.rst b/docs/source/API-Utils.rst deleted file mode 100644 index ce60b5234..000000000 --- a/docs/source/API-Utils.rst +++ /dev/null @@ -1,4 +0,0 @@ -.. include:: _API-Header.rst - -.. automodule:: webots_ros2_core.utils - :members: diff --git a/docs/source/API-WebotsLauncher.rst b/docs/source/API-WebotsLauncher.rst deleted file mode 100644 index 33b2b553a..000000000 --- a/docs/source/API-WebotsLauncher.rst +++ /dev/null @@ -1,5 +0,0 @@ -.. include:: _API-Header.rst - -.. automodule:: webots_ros2_core.webots_launcher - :members: - :show-inheritance: diff --git a/docs/source/API-WebotsNode.rst b/docs/source/API-WebotsNode.rst deleted file mode 100644 index d2b7bb4cb..000000000 --- a/docs/source/API-WebotsNode.rst +++ /dev/null @@ -1,15 +0,0 @@ -.. include:: _API-Header.rst - -.. automodule:: webots_ros2_core.webots_node - :members: - :show-inheritance: - - -.. automodule:: webots_ros2_core.webots_differential_drive_node - :members: - :show-inheritance: - - -.. automodule:: webots_ros2_core.webots_robotic_arm_node - :members: - :show-inheritance: diff --git a/docs/source/_API-Header.rst b/docs/source/_API-Header.rst deleted file mode 100644 index eb91d6151..000000000 --- a/docs/source/_API-Header.rst +++ /dev/null @@ -1,3 +0,0 @@ -> **Note:** _Do not edit these wiki pages as they are automatically generated from the Python docstrings._ -If you edit them, your changes will be lost. -Instead, you should edit the Python files or the `*.rst` files. diff --git a/docs/source/_API-Index.rst b/docs/source/_API-Index.rst deleted file mode 100644 index c195416f8..000000000 --- a/docs/source/_API-Index.rst +++ /dev/null @@ -1,11 +0,0 @@ -API -=== - -.. toctree:: - :maxdepth: 2 - - API-Devices - API-WebotsNode - API-WebotsLauncher - API-JointStatePublisher - API-Utils diff --git a/docs/source/conf.py b/docs/source/conf.py deleted file mode 100644 index 1c8539dc9..000000000 --- a/docs/source/conf.py +++ /dev/null @@ -1,94 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import os -import sys -import datetime -import importlib -from unittest.mock import MagicMock - - -sys.path.insert(0, os.path.abspath('../../webots_ros2_core')) - - -# Mock imports -autodoc_mock_imports = [] -for mod in [ - 'controller', - 'control_msgs', - 'geometry_msgs', - 'launch', - 'launch.action', - 'launch.actions', - 'launch.launch_context', - 'launch.substitution', - 'launch.substitutions', - 'launch_ros', - 'nav_msgs', - 'rclpy', - 'rcl_interfaces', - 'rosgraph_msgs', - 'sensor_msgs', - 'std_msgs', - 'tf2_msgs', - 'tf2_ros', - 'trajectory_msgs', - 'tkinter', - 'webots_ros2_core', - 'webots_ros2_msgs' -]: - try: - importlib.import_module(mod) - except ImportError: - autodoc_mock_imports.append(mod) - - -# Mock inherited classes -MOCK_MODULES = [ - 'rclpy.node', -] - -MOCK_CLASSES = [ - 'Node', -] - - -class Mock(MagicMock): - @classmethod - def __getattr__(cls, name): - if name in MOCK_CLASSES: - return object - return MagicMock() - - -sys.modules.update((mod_name, Mock()) for mod_name in MOCK_MODULES) - - -# Meta data -project = 'webots_ros2' -# deepcode ignore W0622: Ignore W0622 -copyright = '{}, Cyberbotics'.format(datetime.datetime.now().year) -author = 'Cyberbotics' - -# Extensions -extensions = [ - 'sphinx_markdown_builder', - 'sphinx.ext.autodoc', - 'sphinx.ext.napoleon' -] - -# Misc -master_doc = '_API-Index' diff --git a/docs/cover.png b/images/cover.png similarity index 100% rename from docs/cover.png rename to images/cover.png diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 51314a162..7429cb6b8 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.0.0 (2022-XX-XX) +------------------ +* Removed 'webots_ros2_core' package (deprecated). + 2022.1.4 (2022-11-18) ------------------ * Fix the camera focal length in the CameraInfo topic. @@ -21,7 +25,7 @@ Changelog for package webots_ros2 ------------------ * Adapted controllers to communicate with Webots R2022b. * Added feature to import URDF on the fly. -* Add PointCloud2 support for RangeFinder. +* Added PointCloud2 support for RangeFinder. 1.2.3 (2022-06-01) ------------------ diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml index bcfdca8a5..6cc4c43ca 100644 --- a/webots_ros2/package.xml +++ b/webots_ros2/package.xml @@ -15,7 +15,6 @@ std_msgs builtin_interfaces webots_ros2_control - webots_ros2_core webots_ros2_driver webots_ros2_epuck webots_ros2_importer diff --git a/webots_ros2_core/CHANGELOG.rst b/webots_ros2_core/CHANGELOG.rst deleted file mode 100644 index 57faee20b..000000000 --- a/webots_ros2_core/CHANGELOG.rst +++ /dev/null @@ -1,48 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package webots_ros2_core -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.2.3 (2022-02-22) ------------------- -* This package shows now deprecation warnings and will be removed with the release of Webots `R2023a`. -* Users of `webots_ros2_core` should migrate to `webots_ros2_driver`. - -1.1.2 (2021-11-03) ------------------- -* This package is now deprecated. - -1.0.4 (2021-01-08) ------------------- -* Improved performance of camera. -* Replaced `tkinter` by a simple command line tools. -* Fixed usage on Windows. -* Introduced notion of minimum and target Webots versions. - -1.0.3 (2020-12-01) ------------------- -* Improved the performance of point cloud publishing by a few times. - -1.0.2 (2020-10-12) ------------------- -* Fixed support for 3D Lidars -* Fixed Webots executable discovery - -1.0.0 (2020-09-01) ------------------- -* Added a universal 'webots_differential_drive_node' node. - -0.0.4 (2020-07-03) ------------------- -* Fixed dependencies issue. - -0.0.3 (2020-06-15) ------------------- -* Added support for multi robots. -* Added a new TfPublisher class to publish transforms of all the Solid nodes of the robot (if the robot `supervisor` field is true). -* Added the possibility to run nodes in synchronized mode (using the 'synchronization' parameter). -* Added better support for differential drive robots ('WebotsDifferentialDriveNode' class). -* Added CameraDevice, LEDDevice and LaserDevice that create correspoding ROS2 topics - -0.0.2 (2019-09-23) ------------------- -* Initial version diff --git a/webots_ros2_core/README.md b/webots_ros2_core/README.md deleted file mode 100644 index 88f8e8c90..000000000 --- a/webots_ros2_core/README.md +++ /dev/null @@ -1,12 +0,0 @@ -# Core - -> **Note:** This package has been deprecated and will be removed with the release of Webots `R2023a`. -> -> Users of `webots_ros2_core` should migrate to `webots_ros2_driver`. - -This package contains essential building blocks for running Webots simulation, such as Webots launcher, ROS2 wrappers for Webots devices and other relevant utils. - -Please use the following links for more details: - -* [Tutorial: Write ROS2 Driver](https://github.com/cyberbotics/webots_ros2/wiki/Deprecated-Tutorial-Write-ROS2-Driver) -* [References](https://github.com/cyberbotics/webots_ros2/wiki/Deprecated-References) diff --git a/webots_ros2_core/launch/robot_launch.py b/webots_ros2_core/launch/robot_launch.py deleted file mode 100644 index 970576dc3..000000000 --- a/webots_ros2_core/launch/robot_launch.py +++ /dev/null @@ -1,148 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Launch Webots and ROS2 driver.""" - -import os -import launch -from launch import LaunchDescription -from launch.actions import RegisterEventHandler, EmitEvent, DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from webots_ros2_core.utils import ControllerLauncher -from webots_ros2_core.webots_launcher import WebotsLauncher - - -ARGUMENTS = [ - DeclareLaunchArgument( - 'synchronization', - default_value='False', - description='If `False` robot.step() will be automatically called.' - ), - DeclareLaunchArgument( - 'package', - default_value='webots_ros2_core', - description='The Package in which the node executable can be found.' - ), - DeclareLaunchArgument( - 'executable', - default_value='webots_node', - description='The name of the executable to find if a package is provided or otherwise a path to the executable to run.' - ), - DeclareLaunchArgument( - 'world', - description='Path to Webots\' world file. Make sure `controller` field is set to ``.' - ), - DeclareLaunchArgument( - 'gui', - default_value='True', - description='Whether to start GUI or not.' - ), - DeclareLaunchArgument( - 'mode', - default_value='realtime', - description='Choose the startup mode (it must be either `pause`, `realtime`, `run` or `fast`).' - ), - DeclareLaunchArgument( - 'publish_tf', - default_value='True', - description='Whether to publish transforms (tf)' - ), - DeclareLaunchArgument( - 'node_parameters', - description='Path to ROS parameters file that will be passed to the robot node', - default_value=os.devnull - ), - DeclareLaunchArgument( - 'robot_name', - description='The name of the robot (has to be the same as in Webots)', - default_value='' - ), - DeclareLaunchArgument( - 'node_name', - description='The name of the ROS node that interacts with Webots', - default_value='webots_driver' - ), - DeclareLaunchArgument( - 'use_sim_time', - description='Whether to use the simulation (Webots) time', - default_value='True' - ) -] - - -def generate_launch_description(): - synchronization = LaunchConfiguration('synchronization') - package = LaunchConfiguration('package') - executable = LaunchConfiguration('executable') - world = LaunchConfiguration('world') - gui = LaunchConfiguration('gui') - mode = LaunchConfiguration('mode') - publish_tf = LaunchConfiguration('publish_tf') - node_parameters = LaunchConfiguration('node_parameters') - robot_name = LaunchConfiguration('robot_name') - node_name = LaunchConfiguration('node_name') - use_sim_time = LaunchConfiguration('use_sim_time') - - # Webots - webots = WebotsLauncher( - world=world, - mode=mode, - gui=gui - ) - - # Driver node - controller = ControllerLauncher( - package=package, - executable=executable, - parameters=[ - node_parameters, - { - 'synchronization': synchronization, - 'use_joint_state_publisher': publish_tf - }], - output='screen', - arguments=[ - '--webots-robot-name', robot_name, - '--webots-node-name', node_name - ], - ) - - # Robot state publisher - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - output='screen', - parameters=[{ - 'robot_description': '', - 'use_sim_time': use_sim_time - }], - condition=launch.conditions.IfCondition(publish_tf) - ) - - return LaunchDescription(ARGUMENTS + [ - robot_state_publisher, - webots, - controller, - - # Shutdown launch when Webots exits. - RegisterEventHandler( - event_handler=launch.event_handlers.OnProcessExit( - target_action=webots, - on_exit=[EmitEvent(event=launch.events.Shutdown())], - ) - ) - ]) diff --git a/webots_ros2_core/package.xml b/webots_ros2_core/package.xml deleted file mode 100644 index 81c1efe63..000000000 --- a/webots_ros2_core/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - webots_ros2_core - 2022.1.4 - Core interface between Webots and ROS2 - - Cyberbotics - Apache License 2.0 - http://wiki.ros.org/webots_ros2 - https://github.com/cyberbotics/webots_ros2/issues - https://github.com/cyberbotics/webots_ros2 - - rclpy - std_msgs - vision_msgs - builtin_interfaces - webots_ros2_msgs - python3-tk - - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - This package will be removed in Webots R2023a. Users of webots_ros2_core should migrate to webots_ros2_driver. - - diff --git a/webots_ros2_core/resource/webots_ros2_core b/webots_ros2_core/resource/webots_ros2_core deleted file mode 100644 index e69de29bb..000000000 diff --git a/webots_ros2_core/setup.cfg b/webots_ros2_core/setup.cfg deleted file mode 100644 index 66da46018..000000000 --- a/webots_ros2_core/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/webots_ros2_core -[install] -install_scripts=$base/lib/webots_ros2_core diff --git a/webots_ros2_core/setup.py b/webots_ros2_core/setup.py deleted file mode 100644 index e416d5aa9..000000000 --- a/webots_ros2_core/setup.py +++ /dev/null @@ -1,49 +0,0 @@ -"""webots_ros2 package setup file.""" - -from setuptools import setup -import logging - -logging.basicConfig() -webots_ros2_core_logger = logging.getLogger('webots_ros2_core') -webots_ros2_core_logger.warning( - ' This package will be removed with the release of Webots R2023a.' - ' Use "webots_ros2_driver" instead.' - .format_map(locals())) - -package_name = 'webots_ros2_core' -data_files = [] -data_files.append(('share/ament_index/resource_index/packages', ['resource/' + package_name])) -data_files.append(('share/' + package_name, ['package.xml'])) -data_files.append(('share/' + package_name + '/launch', ['launch/robot_launch.py'])) - - -setup( - name=package_name, - version='2022.1.4', - packages=[package_name, package_name + '.devices', package_name + '.math', package_name + '.webots'], - data_files=data_files, - install_requires=['setuptools', 'launch'], - zip_safe=True, - author='Cyberbotics', - author_email='support@cyberbotics.com', - maintainer='Cyberbotics', - maintainer_email='support@cyberbotics.com', - keywords=['ROS', 'Webots', 'Robot', 'Simulation'], - classifiers=[ - 'Intended Audience :: Developers', - 'License :: OSI Approved :: Apache Software License', - 'Programming Language :: Python', - 'Topic :: Software Development', - ], - description='Core interface between Webots and ROS2.', - license='Apache License, Version 2.0', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'webots_differential_drive_node = webots_ros2_core.webots_differential_drive_node:main', - 'webots_robotic_arm_node = webots_ros2_core.webots_robotic_arm_node:main', - 'webots_node = webots_ros2_core.webots_node:main' - ], - 'launch.frontend.launch_extension': ['launch_ros = launch_ros'] - } -) diff --git a/webots_ros2_core/test/test_copyright.py b/webots_ros2_core/test/test_copyright.py deleted file mode 100644 index 33313ea08..000000000 --- a/webots_ros2_core/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test licenses in files.""" - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', '--verbose', '--exclude', 'webots_ros2_core/math/quaternions.py', 'quaternions.py']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_core/test/test_flake8.py b/webots_ros2_core/test/test_flake8.py deleted file mode 100644 index 2614b92c0..000000000 --- a/webots_ros2_core/test/test_flake8.py +++ /dev/null @@ -1,27 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=[ - '--linelength', '128', '--exclude', 'webots_ros2_core/math/quaternions.py' - ]) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_core/test/test_math_utils.py b/webots_ros2_core/test/test_math_utils.py deleted file mode 100644 index 21492fd03..000000000 --- a/webots_ros2_core/test/test_math_utils.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import unittest -from webots_ros2_core.math.interpolation import interpolate_lookup_table - - -TABLE_DESC = [ - 0, 1000, 0, - 0.1, 1000, 0.1, - 0.2, 400, 0.1, - 0.3, 50, 0.1, - 0.37, 30, 0 -] - -TABLE_ASC = [ - 0.37, 30, 0, - 0.3, 50, 0.1, - 0.2, 400, 0.1, - 0.1, 1000, 0.1, - 0, 1000, 0 -] - - -class TestMathUtils(unittest.TestCase): - def test_lookup_desc_interpolation(self): - self.assertAlmostEqual(interpolate_lookup_table(900, TABLE_DESC), 0.117, places=3) - self.assertAlmostEqual(interpolate_lookup_table(31, TABLE_DESC), 0.3665, places=3) - - def test_lookup_desc_edge(self): - self.assertAlmostEqual(interpolate_lookup_table(1000, TABLE_DESC), 0.05, places=3) - self.assertAlmostEqual(interpolate_lookup_table(30, TABLE_DESC), 0.37, places=3) - - def test_lookup_desc_extrapolation(self): - self.assertAlmostEqual(interpolate_lookup_table(2000, TABLE_DESC), 0, places=3) - self.assertAlmostEqual(interpolate_lookup_table(0, TABLE_DESC), 0.475, places=3) - - def test_lookup_asc_interpolation(self): - self.assertAlmostEqual(interpolate_lookup_table(900, TABLE_ASC), 0.117, places=3) - self.assertAlmostEqual(interpolate_lookup_table(31, TABLE_ASC), 0.3665, places=3) - - def test_lookup_asc_edge(self): - self.assertAlmostEqual(interpolate_lookup_table(1000, TABLE_ASC), 0.05, places=3) - self.assertAlmostEqual(interpolate_lookup_table(30, TABLE_ASC), 0.37, places=3) - - def test_lookup_asc_extrapolation(self): - self.assertAlmostEqual(interpolate_lookup_table(2000, TABLE_ASC), 0, places=3) - self.assertAlmostEqual(interpolate_lookup_table(0, TABLE_ASC), 0.475, places=3) - - -if __name__ == '__main__': - unittest.main() diff --git a/webots_ros2_core/test/test_pep257.py b/webots_ros2_core/test/test_pep257.py deleted file mode 100644 index 914b880e5..000000000 --- a/webots_ros2_core/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', '--exclude', 'webots_ros2_core/math/quaternions.py']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_core/webots_ros2_core/.gitignore b/webots_ros2_core/webots_ros2_core/.gitignore deleted file mode 100644 index 225fc6f66..000000000 --- a/webots_ros2_core/webots_ros2_core/.gitignore +++ /dev/null @@ -1 +0,0 @@ -/__pycache__ diff --git a/webots_ros2_core/webots_ros2_core/__init__.py b/webots_ros2_core/webots_ros2_core/__init__.py deleted file mode 100644 index f4bba8eb8..000000000 --- a/webots_ros2_core/webots_ros2_core/__init__.py +++ /dev/null @@ -1,20 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -import logging -logging.basicConfig() -webots_ros2_core_logger = logging.getLogger('webots_ros2_core') -webots_ros2_core_logger.warning( - ' This package will be removed with the release of Webots R2023a.' - ' Use "webots_ros2_driver" instead.' - .format_map(locals())) diff --git a/webots_ros2_core/webots_ros2_core/devices/__init__.py b/webots_ros2_core/webots_ros2_core/devices/__init__.py deleted file mode 100644 index 0054174d7..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. diff --git a/webots_ros2_core/webots_ros2_core/devices/camera_device.py b/webots_ros2_core/webots_ros2_core/devices/camera_device.py deleted file mode 100644 index 96b756dce..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/camera_device.py +++ /dev/null @@ -1,232 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Camera device.""" - -import os -from sensor_msgs.msg import Image, CameraInfo -from vision_msgs.msg import Detection2D, Detection2DArray, ObjectHypothesisWithPose -from geometry_msgs.msg import Point, Quaternion -from std_msgs.msg import ColorRGBA -from webots_ros2_msgs.msg import CameraRecognitionObject, CameraRecognitionObjects -from rclpy.time import Time -from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, QoSReliabilityPolicy, qos_profile_sensor_data -from .sensor_device import SensorDevice -from webots_ros2_core.math.quaternions import axangle2quat - - -class CameraDevice(SensorDevice): - """ - ROS2 wrapper for Webots Camera node. - - Creates suitable ROS2 interface based on Webots [Camera](https://cyberbotics.com/doc/reference/camera) node instance: - - It allows the following functinalities: - - Publishes raw image of type `sensor_msgs/Image` - - Publishes intrinsic camera parameters of type `sensor_msgs/CameraInfo` (latched topic) - - - Parameters - ---------- - node: WebotsNode - The ROS2 node. - device_key: str - Unique identifier of the device used for configuration. - wb_device: Camera - Webots node of type Camera. - params: dict - Inherited from `SensorDevice` - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self._camera_info_publisher = None - self._recognition_publisher = None - self._recognition_webots_publisher = None - self._image_publisher = None - - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - # Create topics - if not self._disable: - self._image_publisher = self._node.create_publisher( - Image, - self._topic_name + '/image_raw', - qos_sensor_reliable - ) - self._camera_info_publisher = self._node.create_publisher( - CameraInfo, - self._topic_name + '/camera_info', - QoSProfile( - depth=1, - reliability=QoSReliabilityPolicy.RELIABLE, - durability=DurabilityPolicy.TRANSIENT_LOCAL, - history=HistoryPolicy.KEEP_LAST, - ) - ) - if self._wb_device.hasRecognition(): - self._recognition_publisher = self._node.create_publisher( - Detection2DArray, - self._topic_name + '/recognitions', - qos_sensor_reliable - ) - self._recognition_webots_publisher = self._node.create_publisher( - CameraRecognitionObjects, - self._topic_name + '/recognitions/webots', - qos_sensor_reliable - ) - - # CameraInfo data - self.__message_info = CameraInfo() - self.__message_info.header.stamp = Time( - seconds=self._node.robot.getTime()).to_msg() - self.__message_info.height = self._wb_device.getHeight() - self.__message_info.width = self._wb_device.getWidth() - self.__message_info.distortion_model = 'plumb_bob' - focal_length = self._wb_device.getFocalLength() - if focal_length == 0: - focal_length = 570.34 # Identical to Orbbec Astra - self.__message_info.d = [0.0, 0.0, 0.0, 0.0, 0.0] - self.__message_info.r = [ - 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] - self.__message_info.k = [ - focal_length, 0.0, self._wb_device.getWidth() / 2, - 0.0, focal_length, self._wb_device.getHeight() / 2, - 0.0, 0.0, 1.0 - ] - self.__message_info.p = [ - focal_length, 0.0, self._wb_device.getWidth() / 2, 0.0, - 0.0, focal_length, self._wb_device.getHeight() / 2, 0.0, - 0.0, 0.0, 1.0, 0.0 - ] - self._camera_info_publisher.publish(self.__message_info) - - # Load parameters - camera_period_param = node.declare_parameter( - wb_device.getName() + '_period', self._timestep) - self._camera_period = camera_period_param.value - - def step(self): - stamp = super().step() - if not stamp: - return - # Publish camera data - if self._image_publisher.get_subscription_count() > 0 or self._always_publish: - self._wb_device.enable(self._timestep) - image = self._wb_device.getImage() - - if image is None: - return - - # Image data - msg = Image() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.height = self._wb_device.getHeight() - msg.width = self._wb_device.getWidth() - msg.is_bigendian = False - msg.step = self._wb_device.getWidth() * 4 - # We pass `data` directly to we avoid using `data` setter. - # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. - # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable - # behavior. - # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. - msg._data = image - msg.encoding = 'bgra8' - self._image_publisher.publish(msg) - - self.__message_info.header.stamp = Time( - seconds=self._node.robot.getTime()).to_msg() - self._camera_info_publisher.publish(self.__message_info) - - if self._wb_device.hasRecognition(): - if (self._recognition_publisher.get_subscription_count() > 0 or - self._recognition_webots_publisher.get_subscription_count() > 0): - self._wb_device.recognitionEnable(self._timestep) - objects = self._wb_device.getRecognitionObjects() - - if objects is None: - return - - # Recognition data - reco_msg = Detection2DArray() - reco_msg_webots = CameraRecognitionObjects() - reco_msg.header.stamp = stamp - reco_msg_webots.header.stamp = stamp - reco_msg.header.frame_id = self._frame_id - reco_msg_webots.header.frame_id = self._frame_id - for obj in objects: - # Getting Object Info - position = Point() - orientation = Quaternion() - position.x = obj.get_position()[0] - position.y = obj.get_position()[1] - position.z = obj.get_position()[2] - axangle = obj.get_orientation() - quat = axangle2quat(axangle[:-1], axangle[-1]) - orientation.w = quat[0] - orientation.x = quat[1] - orientation.y = quat[2] - orientation.z = quat[3] - obj_model = obj.get_model().decode('UTF-8') - obj_center = [float(i) for i in obj.get_position_on_image()] - obj_size = [float(i) for i in obj.get_size_on_image()] - obj_id = obj.get_id() - obj_colors = obj.get_colors() - # Object Info -> Detection2D - reco_obj = Detection2D() - hyp = ObjectHypothesisWithPose() - hyp.id = obj_model - hyp.pose.pose.position = position - hyp.pose.pose.orientation = orientation - reco_obj.results.append(hyp) - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ('foxy', 'galactic'): - reco_obj.bbox.center.x = obj_center[0] - reco_obj.bbox.center.y = obj_center[1] - else: - reco_obj.bbox.center.position.x = obj_center[0] - reco_obj.bbox.center.position.y = obj_center[1] - reco_obj.bbox.size_x = obj_size[0] - reco_obj.bbox.size_y = obj_size[1] - reco_msg.detections.append(reco_obj) - - # Object Info -> CameraRecognitionObject - reco_webots_obj = CameraRecognitionObject() - reco_webots_obj.id = obj_id - reco_webots_obj.model = obj_model - reco_webots_obj.pose.pose.position = position - reco_webots_obj.pose.pose.orientation = orientation - if 'ROS_DISTRO' in os.environ and os.environ['ROS_DISTRO'] in ('foxy', 'galactic'): - reco_webots_obj.bbox.center.x = obj_center[0] - reco_webots_obj.bbox.center.y = obj_center[1] - else: - reco_webots_obj.bbox.center.position.x = obj_center[0] - reco_webots_obj.bbox.center.position.y = obj_center[1] - reco_webots_obj.bbox.size_x = obj_size[0] - reco_webots_obj.bbox.size_y = obj_size[1] - for i in range(0, obj.get_number_of_colors()): - color = ColorRGBA() - color.r = obj_colors[3 * i] - color.g = obj_colors[3 * i + 1] - color.b = obj_colors[3 * i + 2] - reco_webots_obj.colors.append(color) - reco_msg_webots.objects.append(reco_webots_obj) - self._recognition_webots_publisher.publish(reco_msg_webots) - self._recognition_publisher.publish(reco_msg) - else: - self._wb_device.recognitionDisable() - else: - self._wb_device.disable() diff --git a/webots_ros2_core/webots_ros2_core/devices/device.py b/webots_ros2_core/webots_ros2_core/devices/device.py deleted file mode 100644 index 64cc33b41..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/device.py +++ /dev/null @@ -1,34 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -class Device: - def __init__(self, node, device_key, wb_device, params=None): - self._node = node - self._device_key = device_key - self._wb_device = wb_device - self._params = params or {} - - def step(self): - raise NotImplementedError('.step() method is called on every step and it should be implemented.') - - def _create_topic_name(self, wb_device): - return wb_device.getName().replace('-', '_').replace(' ', '_').replace('.', '_') - - def _create_frame_id(self, wb_device): - return self._create_topic_name(wb_device) - - def _get_param(self, param_name, default_value): - param_value = self._params.setdefault(param_name, default_value) - return self._node.declare_parameter(self._device_key + '.' + param_name, param_value).value diff --git a/webots_ros2_core/webots_ros2_core/devices/device_manager.py b/webots_ros2_core/webots_ros2_core/devices/device_manager.py deleted file mode 100644 index 3ad1ff8c8..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/device_manager.py +++ /dev/null @@ -1,144 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Auto discover Webots devices and publish suitable ROS2 topics.""" - -from .camera_device import CameraDevice -from .range_finder_device import RangeFinderDevice -from .led_device import LEDDevice -from .lidar_device import LidarDevice -from .distance_sensor_device import DistanceSensorDevice -from .light_sensor_device import LightSensorDevice -from .robot_device import RobotDevice -from .imu_device import ImuDevice -from .gps_device import GpsDevice -from webots_ros2_core.webots.controller import Node - - -class DeviceManager: - """Discovers Webots devices and creates corresponding ROS2 topics/services.""" - - def __init__(self, node, config=None): - self.__node = node - self.__devices = {} - self.__config = config or {} - self.__wb_devices = {} - - # Find devices - self.__devices['robot'] = RobotDevice(node, 'robot', node.robot, self.__config.get('robot', None)) - for i in range(node.robot.getNumberOfDevices()): - wb_device = node.robot.getDeviceByIndex(i) - device_key = wb_device.getName() - device = None - - # Create ROS2 wrapped device - if wb_device.getNodeType() == Node.CAMERA: - device = CameraDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - if wb_device.getNodeType() == Node.RANGE_FINDER: - device = RangeFinderDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - elif wb_device.getNodeType() == Node.LED: - device = LEDDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - elif wb_device.getNodeType() == Node.LIDAR: - device = LidarDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - elif wb_device.getNodeType() == Node.DISTANCE_SENSOR: - device = DistanceSensorDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - elif wb_device.getNodeType() == Node.LIGHT_SENSOR: - device = LightSensorDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - elif wb_device.getNodeType() == Node.GPS: - device = GpsDevice(node, device_key, wb_device, self.__config.get(device_key, None)) - - # Add device to the list - self.__wb_devices[device_key] = wb_device - if device is not None: - self.__devices[device_key] = device - - # Multi-Webots-device (insert if not configured + create configured) - self.__insert_imu_device() - for device_key in self.__config.keys(): - if self.__is_imu_device(device_key): - self.__devices[device_key] = ImuDevice( - node, - device_key, - self.__get_imu_wb_devices_from_key(device_key), - self.__config.get(device_key, None) - ) - - # Verify parameters - for device_name in self.__config.keys(): - if device_name not in self.__devices.keys(): - self.__node.get_logger().warn( - f'Device `{device_name}` is not considered! The device doesn\'t exist or it is not supported.') - - def step(self): - for device in self.__devices.values(): - device.step() - - def __is_imu_device(self, device_key): - return any(self.__get_imu_wb_devices_from_key(device_key)) - - def __get_imu_wb_devices_from_key(self, device_key): - wb_device_names = device_key.split('+') - - accelerometer = None - inertial_unit = None - gyro = None - - for wb_device_name in wb_device_names: - if wb_device_name in self.__wb_devices: - if self.__wb_devices[wb_device_name].getNodeType() == Node.ACCELEROMETER: - accelerometer = self.__wb_devices[wb_device_name] - elif self.__wb_devices[wb_device_name].getNodeType() == Node.INERTIAL_UNIT: - inertial_unit = self.__wb_devices[wb_device_name] - elif self.__wb_devices[wb_device_name].getNodeType() == Node.GYRO: - gyro = self.__wb_devices[wb_device_name] - - return [accelerometer, gyro, inertial_unit] - - def __insert_imu_device(self): - """Insert Imu device only if there is no Imu device configured and there is only one Imu device in the robot.""" - accelerometers = [] - inertial_units = [] - gyros = [] - - # Ignore everything if any Imu is configured - for config_key in self.__config.keys(): - if self.__is_imu_device(config_key): - return - - # Classify and add to array - for i in range(self.__node.robot.getNumberOfDevices()): - wb_device = self.__node.robot.getDeviceByIndex(i) - if wb_device.getNodeType() == Node.ACCELEROMETER: - accelerometers.append(wb_device) - elif wb_device.getNodeType() == Node.INERTIAL_UNIT: - inertial_units.append(wb_device) - elif wb_device.getNodeType() == Node.GYRO: - gyros.append(wb_device) - - # If there is only one candiate for Imu create a device and insert it to `self.__devices` - if len(accelerometers) <= 1 and len(inertial_units) <= 1 and len(gyros) <= 1 and \ - (len(accelerometers) + len(inertial_units) + len(gyros)) > 0: - imu_wb_devices = [ - accelerometers[0] if len(accelerometers) > 0 else None, - gyros[0] if len(gyros) > 0 else None, - inertial_units[0] if len(inertial_units) > 0 else None - ] - imu_wb_device_name = [wb_device.getName() for wb_device in imu_wb_devices if wb_device] - device_key = '+'.join(imu_wb_device_name) - self.__devices[device_key] = ImuDevice(self.__node, device_key, imu_wb_devices, { - 'topic_name': '/imu', - 'frame_id': imu_wb_device_name[0] - }) diff --git a/webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py b/webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py deleted file mode 100644 index 297dc3732..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/distance_sensor_device.py +++ /dev/null @@ -1,95 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Webots DistanceSensor device wrapper for ROS2.""" - -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from sensor_msgs.msg import Range -from webots_ros2_core.math.interpolation import interpolate_lookup_table -from .sensor_device import SensorDevice - - -class DistanceSensorDevice(SensorDevice): - """ - ROS2 wrapper for Webots DistanceSensor node. - - Creates suitable ROS2 interface based on Webots [DistanceSensor](https://cyberbotics.com/doc/reference/distancesensor) node. - - It allows the following functinalities: - - Publishes range measurements of type `sensor_msgs/Range` - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (DistanceSensor): Webots node of type DistanceSensor. - - Kwargs: - params (dict): Inherited from `SensorDevice` - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self._publisher = None - self._min_range = self.__get_min_value() + self.__get_lower_std() - self._max_range = self.__get_max_value() - self.__get_upper_std() - - # Create topics - if not self._disable: - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - self._publisher = self._node.create_publisher(Range, self._topic_name, - qos_sensor_reliable) - - def __get_max_value(self): - table = self._wb_device.getLookupTable() - return max(table[0], table[-3]) - - def __get_min_value(self): - table = self._wb_device.getLookupTable() - return min(table[0], table[-3]) - - def __get_lower_std(self): - table = self._wb_device.getLookupTable() - if table[0] < table[-3]: - return table[2] * table[0] - return table[-1] * table[-3] - - def __get_upper_std(self): - table = self._wb_device.getLookupTable() - if table[0] > table[-3]: - return table[2] * table[0] - return table[-1] * table[-3] - - def step(self): - stamp = super().step() - if not stamp: - return - - # Publish distance sensor data - if self._publisher.get_subscription_count() > 0 or self._always_publish: - self._wb_device.enable(self._timestep) - msg = Range() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.field_of_view = self._wb_device.getAperture() - msg.min_range = self._min_range - msg.max_range = self._max_range - msg.range = interpolate_lookup_table(self._wb_device.getValue(), self._wb_device.getLookupTable()) - msg.radiation_type = Range.INFRARED - self._publisher.publish(msg) - else: - self._wb_device.disable() diff --git a/webots_ros2_core/webots_ros2_core/devices/gps_device.py b/webots_ros2_core/webots_ros2_core/devices/gps_device.py deleted file mode 100644 index e09178ade..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/gps_device.py +++ /dev/null @@ -1,109 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Webots GPS device wrapper for ROS2.""" - -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from std_msgs.msg import Float32 -from sensor_msgs.msg import NavSatFix, NavSatStatus -from geometry_msgs.msg import PointStamped -from .sensor_device import SensorDevice -from controller import GPS - - -class GpsDevice(SensorDevice): - """ - ROS2 wrapper for Webots GPS node. - - Creates suitable ROS2 interface based on Webots - [GPS](https://cyberbotics.com/doc/reference/gps) node instance: - - It allows the following functinalities: - - Publishes position measurements of type `sensor_msgs::NavSatFix` if WGS84 - - Publishes position measurements of type `geometry_msgs::PointStamped` if LOCAL - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (Gps): Webots node of type GPS. - - Kwargs: - params (dict): Inherited from `SensorDevice` + the following:: - - dict: { - 'timestep': int, # Publish period in ms (default 128ms) - } - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self.__speed_publisher = None - self.__gps_publisher = None - self.__coordinate_system = self._wb_device.getCoordinateSystem() - - # Exit if disabled - if self._disable: - return - - # Change default timestep - self._timestep = 128 - - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - # Create topics - self.__speed_publisher = node.create_publisher( - Float32, self._topic_name + '/speed', qos_sensor_reliable) - - if self.__coordinate_system == GPS.WGS84: - self.__gps_publisher = node.create_publisher( - NavSatFix, self._topic_name + '/gps', qos_sensor_reliable) - else: - self.__gps_publisher = node.create_publisher( - PointStamped, self._topic_name + '/gps', qos_sensor_reliable) - - def step(self): - stamp = super().step() - if not stamp: - return - - if self.__gps_publisher.get_subscription_count() > 0 or \ - self.__speed_publisher.get_subscription_count() > 0 or \ - self._always_publish: - self._wb_device.enable(self._timestep) - msg = Float32() - msg.data = self._wb_device.getSpeed() - self.__speed_publisher.publish(msg) - if self.__coordinate_system == GPS.WGS84: - msg = NavSatFix() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.latitude = self._wb_device.getValues()[0] - msg.longitude = self._wb_device.getValues()[1] - msg.altitude = self._wb_device.getValues()[2] - msg.position_covariance_type = NavSatFix.COVARIANCE_TYPE_UNKNOWN - msg.status.service = NavSatStatus.SERVICE_GPS - self.__gps_publisher.publish(msg) - else: - msg = PointStamped() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.point.x = self._wb_device.getValues()[0] - msg.point.y = self._wb_device.getValues()[1] - msg.point.z = self._wb_device.getValues()[2] - self.__gps_publisher.publish(msg) - else: - self._wb_device.disable() diff --git a/webots_ros2_core/webots_ros2_core/devices/imu_device.py b/webots_ros2_core/webots_ros2_core/devices/imu_device.py deleted file mode 100644 index 6e8cd4b9b..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/imu_device.py +++ /dev/null @@ -1,101 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Webots Accelerometer, Gyro and InertialUnit devices wrapper for ROS2.""" - -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from sensor_msgs.msg import Imu -from webots_ros2_core.math.interpolation import interpolate_lookup_table -from .sensor_device import SensorDevice - - -class ImuDevice(SensorDevice): - """ - ROS2 wrapper for Webots Accelerometer, Gyro and InertialUnit node. - - Creates suitable ROS2 interface based on Webots - [Accelerometer](https://cyberbotics.com/doc/reference/accelerometer), - [Gyro](https://cyberbotics.com/doc/reference/gyro) and - [InertialUnit](https://cyberbotics.com/doc/reference/inertialunit) node instances: - - It allows the following functinalities: - - Combines readings from Accelerometer, Gyro and InertialUnit to ROS topic of type `sensor_msgs/Imu` - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_devices (array): Webots nodes in the following orderd, Accelerometer, Gyro and InertialUnit. - If device is not available None should be put. - - Kwargs: - params (dict): Inherited from `SensorDevice` - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - - self.__accelerometer = wb_device[0] - self.__gyro = wb_device[1] - self.__inertial_unit = wb_device[2] - - # Create topics - self._publisher = None - if not self._disable: - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - self._publisher = self._node.create_publisher(Imu, self._topic_name, - qos_sensor_reliable) - - def __enable_imu(self): - for wb_device in self._wb_device: - if wb_device is not None: - wb_device.enable(self._timestep) - - def __disable_imu(self): - for wb_device in self._wb_device: - if wb_device: - wb_device.disable() - - def step(self): - stamp = super().step() - if not stamp: - return - - if self._publisher.get_subscription_count() > 0 or self._always_publish: - self.__enable_imu() - msg = Imu() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - if self.__accelerometer: - raw_data = self.__accelerometer.getValues() - msg.linear_acceleration.x = interpolate_lookup_table(raw_data[0], self.__accelerometer.getLookupTable()) - msg.linear_acceleration.y = interpolate_lookup_table(raw_data[1], self.__accelerometer.getLookupTable()) - msg.linear_acceleration.z = interpolate_lookup_table(raw_data[2], self.__accelerometer.getLookupTable()) - if self.__gyro: - raw_data = self.__gyro.getValues() - msg.angular_velocity.x = interpolate_lookup_table(raw_data[0], self.__gyro.getLookupTable()) - msg.angular_velocity.y = interpolate_lookup_table(raw_data[1], self.__gyro.getLookupTable()) - msg.angular_velocity.z = interpolate_lookup_table(raw_data[2], self.__gyro.getLookupTable()) - if self.__inertial_unit: - raw_data = self.__inertial_unit.getQuaternion() - msg.orientation.x = raw_data[0] - msg.orientation.y = raw_data[1] - msg.orientation.z = raw_data[2] - msg.orientation.w = raw_data[3] - self._publisher.publish(msg) - else: - self.__disable_imu() diff --git a/webots_ros2_core/webots_ros2_core/devices/led_device.py b/webots_ros2_core/webots_ros2_core/devices/led_device.py deleted file mode 100644 index 71e6b835f..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/led_device.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""LED device.""" - -from std_msgs.msg import Int32 -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from .device import Device - - -class LEDDevice(Device): - """ - ROS2 wrapper for Webots LED node. - - Creates suitable ROS2 interface based on Webots [LED](https://cyberbotics.com/doc/reference/led) node instance: - - It allows the following functinalities: - - Subscribes to `std_msgs/Int32` and controls LEDs on the robot - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (LED): Webots node of type LED. - - Kwargs: - params (dict): Dictionary with configuration options in format of:: - - dict: { - 'topic_name': str, # ROS topic name (default will generated from the sensor name) - } - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - - # Determine default params - self._topic_name = self._get_param('topic_name', self._create_topic_name(wb_device)) - - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - # Create subscribers - self.__led_subscriber = self._node.create_subscription( - Int32, - self._topic_name, - self.__callback, - qos_sensor_reliable - ) - - def __callback(self, msg): - self._wb_device.set(msg.data) - - def step(self): - pass diff --git a/webots_ros2_core/webots_ros2_core/devices/lidar_device.py b/webots_ros2_core/webots_ros2_core/devices/lidar_device.py deleted file mode 100644 index 157fe259f..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/lidar_device.py +++ /dev/null @@ -1,138 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Lidar device.""" - -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from sensor_msgs.msg import LaserScan, PointCloud2, PointField -from tf2_ros import StaticTransformBroadcaster -from geometry_msgs.msg import TransformStamped -from .sensor_device import SensorDevice - - -class LidarDevice(SensorDevice): - """ - ROS2 wrapper for Webots Lidar node. - - Creates suitable ROS2 interface based on Webots [Lidar](https://cyberbotics.com/doc/reference/lidar) node instance: - - It allows the following functinalities: - - Publishes range measurements of type `sensor_msgs/LaserScan` if 2D Lidar is present - - Publishes range measurements of type `sensor_msgs/PointCloud2` if 3D Lidar is present - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (Lidar): Webots node of type Lidar. - - Kwargs: - params (dict): Inherited from `SensorDevice` + the following:: - - dict: { - 'noise': int, # Maximum sensor noise used to compensate the maximum range (default 0.01) - 'timestep': int, # Publish period in ms (default 128ms) - } - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self.__publishers = {} - self.__static_transforms = [] - self.__static_broadcaster = None - self.__noise = self._get_param('noise', 1e-2) - - # Exit if disabled - if self._disable: - return - - # Change default timestep - self._timestep = 128 - - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - # Create topics - if wb_device.getNumberOfLayers() > 1: - wb_device.enablePointCloud() - self.__publisher = node.create_publisher(PointCloud2, self._topic_name, - qos_sensor_reliable) - else: - self.__publisher = node.create_publisher(LaserScan, self._topic_name, - qos_sensor_reliable) - self.__static_broadcaster = StaticTransformBroadcaster(node) - transform_stamped = TransformStamped() - transform_stamped.header.frame_id = self._frame_id - transform_stamped.child_frame_id = self._frame_id + '_rotated' - transform_stamped.transform.rotation.x = 0.5 - transform_stamped.transform.rotation.y = 0.5 - transform_stamped.transform.rotation.z = -0.5 - transform_stamped.transform.rotation.w = 0.5 - self.__static_broadcaster.sendTransform(transform_stamped) - - def step(self): - stamp = super().step() - if not stamp: - return - - if self.__publisher.get_subscription_count() > 0 or self._always_publish: - self._wb_device.enable(self._timestep) - if self._wb_device.getNumberOfLayers() > 1: - self.__publish_point_cloud_data(stamp) - else: - self.__publish_laser_scan_data(stamp) - else: - self._wb_device.disable() - - def __publish_point_cloud_data(self, stamp): - data = self._wb_device.getPointCloud(data_type='buffer') - if data: - msg = PointCloud2() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.height = 1 - msg.width = self._wb_device.getNumberOfPoints() - msg.point_step = 20 - msg.row_step = 20 * self._wb_device.getNumberOfPoints() - msg.is_dense = False - msg.fields = [ - PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), - PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), - PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1) - ] - msg.is_bigendian = False - # We pass `data` directly to we avoid using `data` setter. - # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. - # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable - # behavior. - # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. - msg._data = data - self.__publisher.publish(msg) - - def __publish_laser_scan_data(self, stamp): - """Publish the laser scan topics with up to date value.""" - ranges = self._wb_device.getLayerRangeImage(0) - if ranges: - msg = LaserScan() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id + '_rotated' - msg.angle_min = -0.5 * self._wb_device.getFov() - msg.angle_max = 0.5 * self._wb_device.getFov() - msg.angle_increment = self._wb_device.getFov() / (self._wb_device.getHorizontalResolution() - 1) - msg.scan_time = self._wb_device.getSamplingPeriod() / 1000.0 - msg.range_min = self._wb_device.getMinRange() + self.__noise - msg.range_max = self._wb_device.getMaxRange() - self.__noise - msg.ranges = ranges - self.__publisher.publish(msg) diff --git a/webots_ros2_core/webots_ros2_core/devices/light_sensor_device.py b/webots_ros2_core/webots_ros2_core/devices/light_sensor_device.py deleted file mode 100644 index 418b40cf2..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/light_sensor_device.py +++ /dev/null @@ -1,94 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Webots LightSensor device wrapper for ROS2.""" - -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from sensor_msgs.msg import Illuminance -from webots_ros2_core.math.interpolation import interpolate_lookup_table -from .sensor_device import SensorDevice - - -# https://ieee-dataport.org/open-access/conversion-guide-solar-irradiance-and-lux-illuminance -IRRADIANCE_TO_ILLUMINANCE = 120 - - -class LightSensorDevice(SensorDevice): - """ - ROS2 wrapper for Webots LightSensor node. - - Creates suitable ROS2 interface based on Webots [LightSensor](https://cyberbotics.com/doc/reference/lightsensor) node. - - It allows the following functinalities: - - Publishes range measurements of type `sensor_msgs/Illuminance` - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (LightSensor): Webots node of type LightSensor. - - Kwargs: - params (dict): Inherited from `SensorDevice` - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - - # Create topics - self._publisher = None - if not self._disable: - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - self._publisher = self._node.create_publisher(Illuminance, self._topic_name, - qos_sensor_reliable) - - def __get_variance(self, raw_value): - table = self._wb_device.getLookupTable() - - # Find relative standard deviation in lookup table - relative_std = None - for i in range(0, len(table) - 3, 3): - if table[i+1] < raw_value < table[i+3+1] or table[i+1] > raw_value > table[i+3+1]: - relative_std = table[i+2] - break - if relative_std is None: - if raw_value < table[1]: - relative_std = table[2] - else: - relative_std = table[-1] - - # Calculate variance from the relative standard deviation - std = interpolate_lookup_table(raw_value, self._wb_device.getLookupTable()) * IRRADIANCE_TO_ILLUMINANCE * relative_std - return std**2 - - def step(self): - stamp = super().step() - if not stamp: - return - - # Publish light sensor data - if self._publisher.get_subscription_count() > 0 or self._always_publish: - self._wb_device.enable(self._timestep) - msg = Illuminance() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.illuminance = interpolate_lookup_table(self._wb_device.getValue( - ), self._wb_device.getLookupTable()) * IRRADIANCE_TO_ILLUMINANCE - msg.variance = self.__get_variance(self._wb_device.getValue()) - self._publisher.publish(msg) - else: - self._wb_device.disable() diff --git a/webots_ros2_core/webots_ros2_core/devices/range_finder_device.py b/webots_ros2_core/webots_ros2_core/devices/range_finder_device.py deleted file mode 100644 index 62238ed8c..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/range_finder_device.py +++ /dev/null @@ -1,95 +0,0 @@ -# Copyright 2021 Intelligent Robotics Labs URJC -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""RangeFinder device.""" - -import numpy as np -from sensor_msgs.msg import Image -from rclpy.qos import QoSReliabilityPolicy, qos_profile_sensor_data -from .sensor_device import SensorDevice - - -class RangeFinderDevice(SensorDevice): - """ - ROS2 wrapper for Webots RangeFinder node. - - Creates suitable ROS2 interface based on Webots - [RangeFinder](https://cyberbotics.com/doc/reference/rangefinder) node instance: - - It allows the following functinalities: - - Publishes raw depth image of type `sensor_msgs/Image` - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (RangeFinder): Webots node of type RangeFinder. - - Kwargs: - params (dict): Inherited from `SensorDevice` - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self._image_publisher = None - - qos_sensor_reliable = qos_profile_sensor_data - qos_sensor_reliable.reliability = QoSReliabilityPolicy.RELIABLE - - # Create topics - if not self._disable: - self._image_publisher = self._node.create_publisher( - Image, - self._topic_name + '/image_depth', - qos_sensor_reliable - ) - - # Load parameters - camera_period_param = node.declare_parameter(wb_device.getName() + '_period', self._timestep) - self._camera_period = camera_period_param.value - - def step(self): - stamp = super().step() - if not stamp: - return - # Publish camera data - if self._image_publisher.get_subscription_count() > 0 or self._always_publish: - self._wb_device.enable(self._timestep) - - # Image data - msg = Image() - msg.header.stamp = stamp - msg.header.frame_id = self._frame_id - msg.height = self._wb_device.getHeight() - msg.width = self._wb_device.getWidth() - msg.is_bigendian = False - msg.step = self._wb_device.getWidth() * 4 - - if self._wb_device.getRangeImage() is None: - return - image_array = np.array(self._wb_device.getRangeImage(), dtype="float32") - - # We pass `data` directly to we avoid using `data` setter. - # Otherwise ROS2 converts data to `array.array` which slows down the simulation as it copies memory internally. - # Both, `bytearray` and `array.array`, implement Python buffer protocol, so we should not see unpredictable - # behavior. - # deepcode ignore W0212: Avoid conversion from `bytearray` to `array.array`. - msg._data = image_array.tobytes() - msg.encoding = '32FC1' - - self._image_publisher.publish(msg) - - else: - self._wb_device.disable() diff --git a/webots_ros2_core/webots_ros2_core/devices/robot_device.py b/webots_ros2_core/webots_ros2_core/devices/robot_device.py deleted file mode 100644 index 6366a5312..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/robot_device.py +++ /dev/null @@ -1,94 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Robot device.""" - -import os -import tempfile -from rcl_interfaces.srv import SetParameters -from rcl_interfaces.msg._parameter import Parameter -from rclpy.parameter import ParameterType, ParameterValue -from tf2_ros import StaticTransformBroadcaster -from geometry_msgs.msg import TransformStamped -from .device import Device - - -class RobotDevice(Device): - """ - ROS2 wrapper for Webots Robot node. - - Creates suitable ROS2 interface based on Webots [Robot](https://cyberbotics.com/doc/reference/robot) node. - - It allows the following functinalities: - - Updates `robot_description` parameter of `robot_state_publisher` node based on obtained URDF. - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (Robot): Webots node of type Robot. - - Kwargs: - params (dict): Dictionary with configuration options in format of:: - - dict: { - 'publish_robot_description': bool, # Whether to publish robot description (default True) - 'publish_base_footprint': bool, # Whether to publish `base_footprint` link (default False) - } - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - self.__base_footprint_broadcaster = None - - # Determine default params - params = params or {} - self.__publish_robot_description = self._get_param('publish_robot_description', True) - self.__publish_base_footprint = self._get_param('publish_base_footprint', False) - - # Create robot_description publishers if needed - if self.__publish_robot_description: - urdf = self._wb_device.getUrdf(self.__get_urdf_prefix()) - self.__save_urdf_to_file(urdf) - self.__set_string_param('robot_state_publisher', 'robot_description', urdf) - - if self.__publish_base_footprint: - self.__base_footprint_broadcaster = StaticTransformBroadcaster(self._node) - transform_stamped = TransformStamped() - transform_stamped.header.frame_id = self.__get_urdf_prefix() + 'base_link' - transform_stamped.child_frame_id = self.__get_urdf_prefix() + 'base_footprint' - transform_stamped.transform.rotation.w = 1.0 - self.__base_footprint_broadcaster.sendTransform(transform_stamped) - - def __save_urdf_to_file(self, urdf): - """Write URDF to a file for debugging purposes.""" - filename = 'webots_robot_{}.urdf'.format(self._node.robot.getName().replace(' ', '_').replace('/', '_')) - with open(os.path.join(tempfile.gettempdir(), filename), 'w') as urdf_file: - urdf_file.write(urdf) - - def __set_string_param(self, node, name, value): - self.cli = self._node.create_client(SetParameters, self._node.get_namespace() + node + '/set_parameters') - self.cli.wait_for_service(timeout_sec=1) - req = SetParameters.Request() - param_value = ParameterValue(string_value=value, type=ParameterType.PARAMETER_STRING) - param = Parameter(name=name, value=param_value) - req.parameters.append(param) - self.cli.call_async(req) - - def __get_urdf_prefix(self): - return self._node.get_namespace()[1:].replace('/', '_') - - def step(self): - pass diff --git a/webots_ros2_core/webots_ros2_core/devices/sensor_device.py b/webots_ros2_core/webots_ros2_core/devices/sensor_device.py deleted file mode 100644 index 3d6ae521d..000000000 --- a/webots_ros2_core/webots_ros2_core/devices/sensor_device.py +++ /dev/null @@ -1,67 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Webots generic sensor device wrapper for ROS2.""" - -from rclpy.time import Time -from .device import Device - - -class SensorDevice(Device): - """ - ROS2 wrapper for Webots sensor nodes. - - Args: - ---- - node (WebotsNode): The ROS2 node. - device_key (str): Unique identifier of the device used for configuration. - wb_device (Device): Webots device node. - - Kwargs: - params (dict): Dictionary with configuration options in format of:: - - dict: { - 'topic_name': str, # ROS topic name (default will generated from the sensor name) - 'timestep': int, # Publish period in ms (default is equal to robot's timestep) - 'disable': bool, # Whether to create ROS interface for this sensor (default false) - 'always_publish': bool, # Publish even if there are no subscribers (default false) - 'frame_id': str, # Value for `header.frame_id` field (default is the same as device name) - } - - """ - - def __init__(self, node, device_key, wb_device, params=None): - super().__init__(node, device_key, wb_device, params) - - self._last_update = -1 - - reference_device = [single_wb_device for single_wb_device in wb_device if single_wb_device][0] if isinstance( - wb_device, list) else wb_device - - # Determine default params - self._topic_name = self._get_param('topic_name', self._create_topic_name(reference_device)) - self._timestep = self._get_param('timestep', int(node.robot.getBasicTimeStep())) - self._disable = self._get_param('disable', False) - self._always_publish = self._get_param('always_publish', False) - self._frame_id = self._get_param('frame_id', reference_device.getName()) - - def step(self): - if self._disable: - return None - - if self._node.robot.getTime() - self._last_update < self._timestep / 1e3: - return None - self._last_update = self._node.robot.getTime() - - return Time(seconds=self._node.robot.getTime()).to_msg() diff --git a/webots_ros2_core/webots_ros2_core/joint_state_publisher.py b/webots_ros2_core/webots_ros2_core/joint_state_publisher.py deleted file mode 100644 index d43c5d7fe..000000000 --- a/webots_ros2_core/webots_ros2_core/joint_state_publisher.py +++ /dev/null @@ -1,78 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Joint state publisher.""" - -from sensor_msgs.msg import JointState -from rclpy.time import Time -from webots_ros2_core.webots.controller import Node - - -class JointStatePublisher: - """ - Publishes joint states. - - Discovers all joints with positional sensors and publishes corresponding ROS2 messages of type - [`sensor_msgs/JointState`](https://github.com/ros2/common_interfaces/blob/master/sensor_msgs/msg/JointState.msg). - - Args: - ---- - robot (WebotsNode): Webots Robot node. - jointPrefix (str): Prefix to all joint names. - node (Node): ROS2 node. - - """ - - def __init__(self, robot, joint_prefix, node, frame_id='joint_states'): - """Initialize the position sensors and the topic.""" - self.__robot = robot - self.__frame_id = frame_id - self.__joint_prefix = joint_prefix - self.__node = node - self.__sensors = [] - self.__timestep = int(robot.getBasicTimeStep()) - self.__last_joint_states = None - self.__previous_time = 0 - self.__previous_position = [] - self.__joint_names = [] - - for i in range(robot.getNumberOfDevices()): - device = robot.getDeviceByIndex(i) - if device.getNodeType() == Node.POSITION_SENSOR: - motor = device.getMotor() - name = motor.getName() if motor is not None else device.getName() - self.__joint_names.append(name) - self.__sensors.append(device) - self.__previous_position.append(0) - device.enable(self.__timestep) - self.__publisher = self.__node.create_publisher(JointState, 'joint_states', 1) - - def publish(self): - """Publish the 'joint_states' topic with up to date value.""" - msg = JointState() - msg.header.stamp = Time(seconds=self.__robot.getTime()).to_msg() - msg.header.frame_id = self.__frame_id - msg.name = [s + self.__joint_prefix for s in self.__joint_names] - msg.position = [] - time_difference = self.__robot.getTime() - self.__previous_time - for i in range(len(self.__sensors)): - value = self.__sensors[i].getValue() - msg.position.append(value) - msg.velocity.append((value - self.__previous_position[i]) / - time_difference if time_difference > 0 else 0.0) - self.__previous_position[i] = value - msg.effort = [0.0] * 6 - self.__publisher.publish(msg) - self.__last_joint_states = msg - self.__previous_time = self.__robot.getTime() diff --git a/webots_ros2_core/webots_ros2_core/math/README.md b/webots_ros2_core/webots_ros2_core/math/README.md deleted file mode 100644 index 07c72cb3f..000000000 --- a/webots_ros2_core/webots_ros2_core/math/README.md +++ /dev/null @@ -1 +0,0 @@ -The files in this folder are coming from: https://github.com/matthew-brett/transforms3d diff --git a/webots_ros2_core/webots_ros2_core/math/__init__.py b/webots_ros2_core/webots_ros2_core/math/__init__.py deleted file mode 100644 index 0054174d7..000000000 --- a/webots_ros2_core/webots_ros2_core/math/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. diff --git a/webots_ros2_core/webots_ros2_core/math/interpolation.py b/webots_ros2_core/webots_ros2_core/math/interpolation.py deleted file mode 100644 index 3497d140e..000000000 --- a/webots_ros2_core/webots_ros2_core/math/interpolation.py +++ /dev/null @@ -1,63 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -def interpolate_function(value, start_x, start_y, end_x, end_y, ascending=None): - if end_x - start_x == 0: - if (ascending and value < start_x) or (not ascending and value > start_x): - return start_y - elif (ascending and value > start_x) or (not ascending and value < start_x): - return end_y - else: - return start_y + (end_y - start_y) / 2 - slope = (end_y - start_y) / (end_x - start_x) - return slope * (value - start_x) + start_y - - -def interpolate_lookup_table(value, table): - if not table: - return value - - # Interpolate - for i in range(int(len(table) / 3) - 1): - if (value < table[i * 3 + 1] and value >= table[(i + 1) * 3 + 1]) or \ - (value >= table[i * 3 + 1] and value < table[(i + 1) * 3 + 1]): - return interpolate_function( - value, - table[i * 3 + 1], - table[i * 3], - table[(i + 1) * 3 + 1], - table[(i + 1) * 3] - ) - - # Extrapolate (we assume that the table is sorted, order is irrelevant) - ascending = (table[1] < table[len(table) - 1*3 + 1]) - if (ascending and value >= table[1]) or (not ascending and value < table[1]): - return interpolate_function( - value, - table[len(table) - 2 * 3 + 1], - table[len(table) - 2 * 3 + 0], - table[len(table) - 1 * 3 + 1], - table[len(table) - 1 * 3 + 0], - ascending - ) - else: - return interpolate_function( - value, - table[1], - table[0], - table[3 + 1], - table[3], - ascending - ) diff --git a/webots_ros2_core/webots_ros2_core/math/quaternions.py b/webots_ros2_core/webots_ros2_core/math/quaternions.py deleted file mode 100644 index 950b06d1d..000000000 --- a/webots_ros2_core/webots_ros2_core/math/quaternions.py +++ /dev/null @@ -1,585 +0,0 @@ -'''Functions to operate on, or return, quaternions. - -Quaternions here consist of 4 values ``w, x, y, z``, where ``w`` is the -real (scalar) part, and ``x, y, z`` are the complex (vector) part. - -Note - rotation matrices here apply to column vectors, that is, -they are applied on the left of the vector. For example: - ->>> import numpy as np ->>> q = [0, 1, 0, 0] # 180 degree rotation around axis 0 ->>> M = quat2mat(q) # from this module ->>> vec = np.array([1, 2, 3]).reshape((3,1)) # column vector ->>> tvec = np.dot(M, vec) - -Terms used in function names: - -* *mat* : array shape (3, 3) (3D non-homogenous coordinates) -* *aff* : affine array shape (4, 4) (3D homogenous coordinates) -* *quat* : quaternion shape (4,) -* *axangle* : rotations encoded by axis vector and angle scalar -''' - -import math -import numpy as np - -_MAX_FLOAT = np.maximum_sctype(np.float) -_FLOAT_EPS = np.finfo(np.float).eps - - -def fillpositive(xyz, w2_thresh=None): - ''' Compute unit quaternion from last 3 values - - Parameters - ---------- - xyz : iterable - iterable containing 3 values, corresponding to quaternion x, y, z - w2_thresh : None or float, optional - threshold to determine if w squared is really negative. - If None (default) then w2_thresh set equal to - ``-np.finfo(xyz.dtype).eps``, if possible, otherwise - ``-np.finfo(np.float).eps`` - - Returns - ------- - wxyz : array shape (4,) - Full 4 values of quaternion - - Notes - ----- - If w, x, y, z are the values in the full quaternion, assumes w is - positive. - - Gives error if w*w is estimated to be negative - - w = 0 corresponds to a 180 degree rotation - - The unit quaternion specifies that np.dot(wxyz, wxyz) == 1. - - If w is positive (assumed here), w is given by: - - w = np.sqrt(1.0-(x*x+y*y+z*z)) - - w2 = 1.0-(x*x+y*y+z*z) can be near zero, which will lead to - numerical instability in sqrt. Here we use the system maximum - float type to reduce numerical instability - - Examples - -------- - >>> import numpy as np - >>> wxyz = fillpositive([0,0,0]) - >>> np.all(wxyz == [1, 0, 0, 0]) - True - >>> wxyz = fillpositive([1,0,0]) # Corner case; w is 0 - >>> np.all(wxyz == [0, 1, 0, 0]) - True - >>> np.dot(wxyz, wxyz) - 1.0 - ''' - # Check inputs (force error if < 3 values) - if len(xyz) != 3: - raise ValueError('xyz should have length 3') - # If necessary, guess precision of input - if w2_thresh is None: - try: # trap errors for non-array, integer array - w2_thresh = -np.finfo(xyz.dtype).eps * 3 - except (AttributeError, ValueError): - w2_thresh = -_FLOAT_EPS * 3 - # Use maximum precision - xyz = np.asarray(xyz, dtype=_MAX_FLOAT) - # Calculate w - w2 = 1.0 - np.dot(xyz, xyz) - if w2 < 0: - if w2 < w2_thresh: - raise ValueError('w2 should be positive, but is %e' % w2) - w = 0 - else: - w = np.sqrt(w2) - return np.r_[w, xyz] - - -def quat2mat(q): - ''' Calculate rotation matrix corresponding to quaternion - - Parameters - ---------- - q : 4 element array-like - - Returns - ------- - M : (3,3) array - Rotation matrix corresponding to input quaternion *q* - - Notes - ----- - Rotation matrix applies to column vectors, and is applied to the - left of coordinate vectors. The algorithm here allows quaternions that - have not been normalized. - - References - ---------- - Algorithm from http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion - - Examples - -------- - >>> import numpy as np - >>> M = quat2mat([1, 0, 0, 0]) # Identity quaternion - >>> np.allclose(M, np.eye(3)) - True - >>> M = quat2mat([0, 1, 0, 0]) # 180 degree rotn around axis 0 - >>> np.allclose(M, np.diag([1, -1, -1])) - True - ''' - w, x, y, z = q - Nq = w*w + x*x + y*y + z*z - if Nq < _FLOAT_EPS: - return np.eye(3) - s = 2.0/Nq - X = x*s - Y = y*s - Z = z*s - wX = w*X; wY = w*Y; wZ = w*Z - xX = x*X; xY = x*Y; xZ = x*Z - yY = y*Y; yZ = y*Z; zZ = z*Z - return np.array( - [[ 1.0-(yY+zZ), xY-wZ, xZ+wY ], - [ xY+wZ, 1.0-(xX+zZ), yZ-wX ], - [ xZ-wY, yZ+wX, 1.0-(xX+yY) ]]) - - -def mat2quat(M): - ''' Calculate quaternion corresponding to given rotation matrix - - Method claimed to be robust to numerical errors in `M`. - - Constructs quaternion by calculating maximum eigenvector for matrix - ``K`` (constructed from input `M`). Although this is not tested, a maximum - eigenvalue of 1 corresponds to a valid rotation. - - A quaternion ``q*-1`` corresponds to the same rotation as ``q``; thus the - sign of the reconstructed quaternion is arbitrary, and we return - quaternions with positive w (q[0]). - - See notes. - - Parameters - ---------- - M : array-like - 3x3 rotation matrix - - Returns - ------- - q : (4,) array - closest quaternion to input matrix, having positive q[0] - - References - ---------- - * http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion - * Bar-Itzhack, Itzhack Y. (2000), "New method for extracting the - quaternion from a rotation matrix", AIAA Journal of Guidance, - Control and Dynamics 23(6):1085-1087 (Engineering Note), ISSN - 0731-5090 - - Examples - -------- - >>> import numpy as np - >>> q = mat2quat(np.eye(3)) # Identity rotation - >>> np.allclose(q, [1, 0, 0, 0]) - True - >>> q = mat2quat(np.diag([1, -1, -1])) - >>> np.allclose(q, [0, 1, 0, 0]) # 180 degree rotn around axis 0 - True - - Notes - ----- - http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion - - Bar-Itzhack, Itzhack Y. (2000), "New method for extracting the - quaternion from a rotation matrix", AIAA Journal of Guidance, - Control and Dynamics 23(6):1085-1087 (Engineering Note), ISSN - 0731-5090 - - ''' - # Qyx refers to the contribution of the y input vector component to - # the x output vector component. Qyx is therefore the same as - # M[0,1]. The notation is from the Wikipedia article. - Qxx, Qyx, Qzx, Qxy, Qyy, Qzy, Qxz, Qyz, Qzz = M.flat - # Fill only lower half of symmetric matrix - K = np.array([ - [Qxx - Qyy - Qzz, 0, 0, 0 ], - [Qyx + Qxy, Qyy - Qxx - Qzz, 0, 0 ], - [Qzx + Qxz, Qzy + Qyz, Qzz - Qxx - Qyy, 0 ], - [Qyz - Qzy, Qzx - Qxz, Qxy - Qyx, Qxx + Qyy + Qzz]] - ) / 3.0 - # Use Hermitian eigenvectors, values for speed - vals, vecs = np.linalg.eigh(K) - # Select largest eigenvector, reorder to w,x,y,z quaternion - q = vecs[[3, 0, 1, 2], np.argmax(vals)] - # Prefer quaternion with positive w - # (q * -1 corresponds to same rotation as q) - if q[0] < 0: - q *= -1 - return q - - -def qmult(q1, q2): - ''' Multiply two quaternions - - Parameters - ---------- - q1 : 4 element sequence - q2 : 4 element sequence - - Returns - ------- - q12 : shape (4,) array - - Notes - ----- - See : http://en.wikipedia.org/wiki/Quaternions#Hamilton_product - ''' - w1, x1, y1, z1 = q1 - w2, x2, y2, z2 = q2 - w = w1*w2 - x1*x2 - y1*y2 - z1*z2 - x = w1*x2 + x1*w2 + y1*z2 - z1*y2 - y = w1*y2 + y1*w2 + z1*x2 - x1*z2 - z = w1*z2 + z1*w2 + x1*y2 - y1*x2 - return np.array([w, x, y, z]) - - -def qconjugate(q): - ''' Conjugate of quaternion - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - conjq : array shape (4,) - w, i, j, k of conjugate of `q` - ''' - return np.array(q) * np.array([1.0, -1, -1, -1]) - - -def qnorm(q): - ''' Return norm of quaternion - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - n : scalar - quaternion norm - - Notes - ----- - http://mathworld.wolfram.com/QuaternionNorm.html - ''' - return np.sqrt(np.dot(q, q)) - - -def qisunit(q): - ''' Return True is this is very nearly a unit quaternion ''' - return np.allclose(qnorm(q), 1) - - -def qinverse(q): - ''' Return multiplicative inverse of quaternion `q` - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - invq : array shape (4,) - w, i, j, k of quaternion inverse - ''' - return qconjugate(q) / qnorm(q) - - -def qeye(dtype = np.float): - ''' Return identity quaternion ''' - return np.array([1.0,0,0,0], dtype = dtype) - - -def qexp(q): - ''' Return exponential of quaternion - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - q_exp : array shape (4,) - The quaternion exponential - - Notes - ----- - See: - - * https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power - * https://math.stackexchange.com/questions/1030737/exponential-function-of-quaternion-derivation - ''' - q = np.array(q) # to ensure there is a dtype - w, v = q[0], q[1:] - norm = np.sqrt(np.dot(v, v)) - result = np.zeros((4,), q.dtype) - - if norm == 0.: - return qeye(q.dtype) - - result[0] = np.cos(norm) - result[1:] = np.sin(norm)/norm * v - return result * np.exp(w) - - -def qlog(q): - ''' Return natural logarithm of quaternion - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - q_log : array shape (4,) - Natual logarithm of quaternion - - Notes - ----- - See: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power - ''' - q = np.array(q) # To ensure there is a dtype - qnorm_ = qnorm(q) - if qnorm_ == 0.: - return qeye(q.dtype) - - w, v = q[0], q[1:] - vnorm = np.sqrt(np.dot(v, v)) - result = np.zeros((4,), q.dtype) - - if vnorm == 0.: - return qeye(q.dtype) - - result[0] = np.log(qnorm_) - result[1:] = v/vnorm * np.arccos(w/qnorm_) - return result - - -def qpow(q, n): - ''' Return the power of quaternion - - Parameters - ---------- - q : 4 element sequence - w, i, j, k of quaternion - n : a real number - - Returns - ------- - q_pow : array shape (4,) - The quaternion `q` to `n`th power. - - Notes - ----- - See: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power - ''' - q = np.array(q) # To ensure there is a dtype. - qnorm_ = qnorm(q) - - if qnorm_ == 0.: - return qeye(q.dtype) - - w, v = q[0], q[1:] - - nnorm = np.sqrt(np.dot(v, v)) - result = np.zeros((4,), q.dtype) - - if nnorm == 0.: - return qeye(q.dtype) - - theta = np.arccos(w/qnorm_) - n_hat = v/nnorm - - result[0] = np.cos(n*theta) - result[1:] = n_hat * np.sin(n*theta) - return result * np.power(qnorm_, n) - - -def rotate_vector(v, q): - ''' Apply transformation in quaternion `q` to vector `v` - - Parameters - ---------- - v : 3 element sequence - 3 dimensional vector - q : 4 element sequence - w, i, j, k of quaternion - - Returns - ------- - vdash : array shape (3,) - `v` rotated by quaternion `q` - - Notes - ----- - See: http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Describing_rotations_with_quaternions - ''' - varr = np.zeros((4,)) - varr[1:] = v - return qmult(q, qmult(varr, qconjugate(q)))[1:] - - -def nearly_equivalent(q1, q2, rtol=1e-5, atol=1e-8): - ''' Returns True if `q1` and `q2` give near equivalent transforms - - `q1` may be nearly numerically equal to `q2`, or nearly equal to `q2` * -1 - (because a quaternion multiplied by -1 gives the same transform). - - Parameters - ---------- - q1 : 4 element sequence - w, x, y, z of first quaternion - q2 : 4 element sequence - w, x, y, z of second quaternion - - Returns - ------- - equiv : bool - True if `q1` and `q2` are nearly equivalent, False otherwise - - Examples - -------- - >>> q1 = [1, 0, 0, 0] - >>> nearly_equivalent(q1, [0, 1, 0, 0]) - False - >>> nearly_equivalent(q1, [1, 0, 0, 0]) - True - >>> nearly_equivalent(q1, [-1, 0, 0, 0]) - True - ''' - q1 = np.array(q1) - q2 = np.array(q2) - if np.allclose(q1, q2, rtol, atol): - return True - return np.allclose(q1 * -1, q2, rtol, atol) - - -def axangle2quat(vector, theta, is_normalized=False): - ''' Quaternion for rotation of angle `theta` around `vector` - - Parameters - ---------- - vector : 3 element sequence - vector specifying axis for rotation. - theta : scalar - angle of rotation in radians. - is_normalized : bool, optional - True if vector is already normalized (has norm of 1). Default - False. - - Returns - ------- - quat : 4 element sequence of symbols - quaternion giving specified rotation - - Examples - -------- - >>> q = axangle2quat([1, 0, 0], np.pi) - >>> np.allclose(q, [0, 1, 0, 0]) - True - - Notes - ----- - Formula from http://mathworld.wolfram.com/EulerParameters.html - ''' - vector = np.array(vector) - if not is_normalized: - # Cannot divide in-place because input vector may be integer type, - # whereas output will be float type; this may raise an error in versions - # of numpy > 1.6.1 - vector = vector / math.sqrt(np.dot(vector, vector)) - t2 = theta / 2.0 - st2 = math.sin(t2) - return np.concatenate(([math.cos(t2)], - vector * st2)) - - -def quat2axangle(quat, identity_thresh=None): - ''' Convert quaternion to rotation of angle around axis - - Parameters - ---------- - quat : 4 element sequence - w, x, y, z forming quaternion. - identity_thresh : None or scalar, optional - Threshold below which the norm of the vector part of the quaternion (x, - y, z) is deemed to be 0, leading to the identity rotation. None (the - default) leads to a threshold estimated based on the precision of the - input. - - Returns - ------- - theta : scalar - angle of rotation. - vector : array shape (3,) - axis around which rotation occurs. - - Examples - -------- - >>> vec, theta = quat2axangle([0, 1, 0, 0]) - >>> vec - array([1., 0., 0.]) - >>> np.allclose(theta, np.pi) - True - - If this is an identity rotation, we return a zero angle and an arbitrary - vector: - - >>> quat2axangle([1, 0, 0, 0]) - (array([1., 0., 0.]), 0.0) - - If any of the quaternion values are not finite, we return a NaN in the - angle, and an arbitrary vector: - - >>> quat2axangle([1, np.inf, 0, 0]) - (array([1., 0., 0.]), nan) - - Notes - ----- - A quaternion for which x, y, z are all equal to 0, is an identity rotation. - In this case we return a 0 angle and an arbitrary vector, here [1, 0, 0]. - - The algorithm allows for quaternions that have not been normalized. - ''' - w, x, y, z = quat - Nq = w * w + x * x + y * y + z * z - if not np.isfinite(Nq): - return np.array([1.0, 0, 0]), float('nan') - if identity_thresh is None: - try: - identity_thresh = np.finfo(Nq.type).eps * 3 - except (AttributeError, ValueError): # Not a numpy type or not float - identity_thresh = _FLOAT_EPS * 3 - if Nq < _FLOAT_EPS ** 2: # Results unreliable after normalization - return np.array([1.0, 0, 0]), 0.0 - if Nq != 1: # Normalize if not normalized - s = math.sqrt(Nq) - w, x, y, z = w / s, x / s, y / s, z / s - len2 = x * x + y * y + z * z - if len2 < identity_thresh ** 2: - # if vec is nearly 0,0,0, this is an identity rotation - return np.array([1.0, 0, 0]), 0.0 - # Make sure w is not slightly above 1 or below -1 - theta = 2 * math.acos(max(min(w, 1), -1)) - return np.array([x, y, z]) / math.sqrt(len2), theta diff --git a/webots_ros2_core/webots_ros2_core/trajectory_follower.py b/webots_ros2_core/webots_ros2_core/trajectory_follower.py deleted file mode 100644 index 09e35cd44..000000000 --- a/webots_ros2_core/webots_ros2_core/trajectory_follower.py +++ /dev/null @@ -1,220 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Implementation of the 'follow_joint_trajectory' ROS action.""" - -import math -import time -from control_msgs.action import FollowJointTrajectory -from trajectory_msgs.msg import JointTrajectoryPoint -from rclpy.duration import Duration -from rclpy.action import ActionServer, CancelResponse, GoalResponse -from webots_ros2_core.webots.controller import Node - - -def to_s(duration): - return Duration.from_msg(duration).nanoseconds / 1e9 - - -class TrajectoryFollower: - """Create and handle the action 'follow_joint_trajectory' server.""" - - def __init__(self, robot, node, joint_prefix, controller_name): - self.__robot = robot - self.__node = node - self.__timestep = int(robot.getBasicTimeStep()) - - # Config - self.__default_tolerance = 0.05 - - # Parse motor and position sensors - self.__motors = {} - for i in list(range(robot.getNumberOfDevices())): - device = robot.getDeviceByIndex(i) - if device.getNodeType() in [Node.LINEAR_MOTOR, Node.ROTATIONAL_MOTOR]: - name = device.getName() - if device.getPositionSensor() is None: - self.__node.get_logger().warn(f'Motor `{name}` doesn\'t have any position sensor.') - else: - self.__motors[joint_prefix + name] = device - device.getPositionSensor().enable(self.__timestep) - - # Initialize trajectory list and action server - self.__current_point_index = 1 - self.__start_time = None - self.__goal = None - self.__mode = None - self.__tolerances = {} - self.__server = ActionServer( - self.__node, - FollowJointTrajectory, - controller_name + '/follow_joint_trajectory', - execute_callback=self.__on_update, - goal_callback=self.__on_goal, - cancel_callback=self.__on_cancel, - handle_accepted_callback=self.__on_goal_accepted - ) - - def log(self, *args): - self.__node.get_logger().warn(' '.join([str(arg) for arg in args])) - - def __on_goal_accepted(self, goal_handle): - goal_handle.execute() - - def __on_goal(self, goal_handle): - """Handle a new goal trajectory command.""" - # Reject if joints don't match - for name in goal_handle.trajectory.joint_names: - if name not in self.__motors.keys(): - joint_names = ', '.join(goal_handle.trajectory.joint_names) - self.__node.get_logger().error(f'Received a goal with incorrect joint names: ({joint_names})') - return GoalResponse.REJECT - - # Reject if infinity or NaN - for point in goal_handle.trajectory.points: - for position, velocity in zip(point.positions, point.velocities): - if math.isinf(position) or math.isnan(position) or math.isinf(velocity) or math.isnan(velocity): - self.__node.get_logger().error('Received a goal with infinites or NaNs') - return GoalResponse.REJECT - - # Reject if joints are already controlled - if self.__goal is not None: - self.__node.get_logger().error('Cannot accept multiple goals') - return GoalResponse.REJECT - - # Store goal data - self.__goal = goal_handle - self.__current_point_index = 1 - self.__start_time = self.__robot.getTime() - - for tolerance in self.__goal.goal_tolerance: - self.__tolerances[tolerance.name] = tolerance.position - for name in self.__goal.trajectory.joint_names: - if name not in self.__tolerances.keys(): - self.__tolerances[name] = self.__default_tolerance - - self.__mode = 'velocity' - for point in self.__goal.trajectory.points: - if to_s(point.time_from_start) != 0: - self.__mode = 'time' - break - - # If a user forget the initial position - if to_s(self.__goal.trajectory.points[0].time_from_start) != 0: - initial_point = JointTrajectoryPoint( - positions=[self.__motors[name].getPositionSensor().getValue() for name in self.__goal.trajectory.joint_names], - time_from_start=Duration().to_msg() - ) - self.__goal.trajectory.points.insert(0, initial_point) - - # Accept the trajectory - self.__node.get_logger().info('Goal Accepted') - return GoalResponse.ACCEPT - - def __on_cancel(self, goal_handle): - """Handle a trajectory cancel command.""" - if self.__goal is not None: - # Stop motors - for name in self.__goal.trajectory.joint_names: - motor = self.__motors[name] - motor.setPosition(motor.getPositionSensor().getValue()) - - self.__goal = None - self.__node.get_logger().info('Goal Canceled') - goal_handle.destroy() - return CancelResponse.ACCEPT - return CancelResponse.REJECT - - def __regulate_velocity_mode(self): - curr_point = self.__goal.trajectory.points[self.__current_point_index] - - for index, name in enumerate(self.__goal.trajectory.joint_names): - self.__set_motor_position(name, curr_point.positions[index]) - - done = TrajectoryFollower.__is_within_tolerance( - curr_point.positions, - [self.__motors[name].getPositionSensor().getValue() for name in self.__goal.trajectory.joint_names], - [self.__tolerances[name] for name in self.__goal.trajectory.joint_names], - ) - if done: - self.__current_point_index += 1 - if self.__current_point_index >= len(self.__goal.trajectory.points): - return True - return False - - def __regulate_time_mode(self): - now = self.__robot.getTime() - prev_point = self.__goal.trajectory.points[self.__current_point_index - 1] - curr_point = self.__goal.trajectory.points[self.__current_point_index] - time_passed = now - self.__start_time - - if time_passed <= to_s(curr_point.time_from_start): - # Linear interpolation - ratio = (time_passed - to_s(prev_point.time_from_start)) /\ - (to_s(curr_point.time_from_start) - to_s(prev_point.time_from_start)) - for index, name in enumerate(self.__goal.trajectory.joint_names): - side = -1 if curr_point.positions[index] < prev_point.positions[index] else 1 - target_position = prev_point.positions[index] + \ - side * ratio * abs(curr_point.positions[index] - prev_point.positions[index]) - self.__set_motor_position(name, target_position) - else: - self.__current_point_index += 1 - if self.__current_point_index >= len(self.__goal.trajectory.points): - return True - return False - - def __set_motor_position(self, name, target_position): - target_position = min(max(target_position, self.__motors[name].getMinPosition()), self.__motors[name].getMaxPosition()) - self.__motors[name].setPosition(target_position) - - async def __on_update(self, goal_handle): - feedback_message = FollowJointTrajectory.Feedback() - feedback_message.joint_names = list(self.__goal.trajectory.joint_names) - - while self.__goal and self.__robot: - done = False - - # Regulate - if self.__mode == 'time': - done = self.__regulate_time_mode() - else: - done = self.__regulate_velocity_mode() - - # Finalize - if done: - self.__node.get_logger().info('Goal Succeeded') - self.__goal = None - goal_handle.succeed() - return FollowJointTrajectory.Result() - - # Publish state - time_passed = self.__robot.getTime() - self.__start_time - feedback_message.actual.positions = [self.__motors[name].getPositionSensor().getValue() - for name in self.__goal.trajectory.joint_names] - feedback_message.actual.time_from_start = Duration(seconds=time_passed).to_msg() - goal_handle.publish_feedback(feedback_message) - - time.sleep(self.__timestep * 1e-3) - - result = FollowJointTrajectory.Result() - result.error_code = result.PATH_TOLERANCE_VIOLATED - return result - - @staticmethod - def __is_within_tolerance(a_vec, b_vec, tol_vec): - """Check if two vectors are equals with a given tolerance.""" - for a, b, tol in zip(a_vec, b_vec, tol_vec): - if abs(a - b) > tol: - return False - return True diff --git a/webots_ros2_core/webots_ros2_core/utils.py b/webots_ros2_core/webots_ros2_core/utils.py deleted file mode 100644 index 8a9b218fe..000000000 --- a/webots_ros2_core/webots_ros2_core/utils.py +++ /dev/null @@ -1,310 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""This launcher simply start Webots.""" - -import os -import re -import sys -import shutil -import tarfile -import argparse -import functools -import subprocess -import urllib.request -from pathlib import Path -from typing import List -from typing import Optional -from launch_ros.actions import Node -from launch.action import Action -from launch.launch_context import LaunchContext - - -MINIMUM_VERSION_STR = 'R2022b' -TARGET_VERSION_STR = 'R2022b' - - -@functools.total_ordering -class WebotsVersion: - def __init__(self, version): - self.version = version - - # Parse - parts = re.findall(r'R(\d*)([a-b]{1})(\s((revision)|(rev))\s(\d){1}){0,1}', version) - self.year = int(parts[0][0]) - self.release = parts[0][1].lower() - self.revision = 0 - if parts[0][-1] != '': - self.revision = int(parts[0][-1]) - - @staticmethod - def from_path(path): - version_file = os.path.join(path, 'resources', 'version.txt') - if not os.path.isfile(version_file): - return None - with open(version_file, 'r') as f: - return WebotsVersion(f.read().strip()) - return None - - @staticmethod - def minimum(): - return WebotsVersion(MINIMUM_VERSION_STR) - - @staticmethod - def target(): - return WebotsVersion(TARGET_VERSION_STR) - - def __eq__(self, other): - if other.get_number() == self.get_number(): - return True - return False - - def __ne__(self, other): - return not self == other - - def __gt__(self, other): - if other.get_number() < self.get_number(): - return True - return False - - def get_number(self): - """Convert the version to a number that can be compared.""" - return self.year * 1e6 + (ord(self.release) - ord('a')) * 1e3 + self.revision - - def __str__(self): - return self.version - - def short(self): - return self.version.replace('revision ', 'rev').replace(' ', '-') - - -def get_webots_home(target_version=None, minimum_version=None, show_warning=False): - # Normalize Webots version - if target_version is not None and isinstance(target_version, str): - target_version = WebotsVersion(target_version) - if minimum_version is not None and isinstance(minimum_version, str): - minimum_version = WebotsVersion(minimum_version) - if target_version is None: - target_version = WebotsVersion.target() - if minimum_version is None: - minimum_version = WebotsVersion.minimum() - - # Search target - path = __get_webots_home(target_version, condition='eq') - if path is not None: - return path - - # Fallback to minumum - path = __get_webots_home(target_version, condition='ge') - if path is not None: - found_version = WebotsVersion.from_path(path) - if show_warning: - print(f'WARNING: Target Webots version `{target_version}` is not found, fallback to `{found_version}`') - return path - - return None - - -def __get_webots_home(target_version, condition='ge'): - """Path to the Webots installation directory.""" - def version_condition(found, target): - if target is None: - return True - if found is None: - return False - if condition == 'eq': - return found == target - elif condition == 'ge': - return found >= target - raise Exception('`condition` can be `eq` or `ge`') - - # Search in the environment variables - environment_variables = ['ROS2_WEBOTS_HOME', 'WEBOTS_HOME'] - for variable in environment_variables: - if variable in os.environ and version_condition(WebotsVersion.from_path(os.environ[variable]), target_version): - os.environ['WEBOTS_HOME'] = os.environ[variable] - return os.environ[variable] - - # Use the 'which' command (Linux and Mac) - try: - where_command = 'where' if sys.platform == 'win32' else 'which' - path = os.path.split(os.path.abspath(subprocess.check_output([where_command, 'webots'])))[0] - path = path.decode('utf-8') - if os.path.isdir(path) and version_condition(WebotsVersion.from_path(path), target_version): - os.environ['WEBOTS_HOME'] = path - return path - except subprocess.CalledProcessError: - # Webots not found - pass - except FileNotFoundError: - # The `which` command not available - pass - - # Look at standard installation pathes - paths = [ - '/usr/local/webots', # Linux default install - '/snap/webots/current/usr/share/webots', # Linux snap install - '/Applications/Webots.app', # macOS default install - 'C:\\Program Files\\Webots', # Windows default install - os.getenv('LOCALAPPDATA', '') + '\\Programs\\Webots' # Windows user install - ] - if target_version is not None: - paths.append(os.path.join(str(Path.home()), '.ros', 'webots' + target_version.short(), 'webots')) - for path in paths: - if os.path.isdir(path) and version_condition(WebotsVersion.from_path(path), target_version): - os.environ['WEBOTS_HOME'] = path - return path - return None - - -def append_webots_lib_to_path(): - """Add the Webots 'lib' folder to the library path.""" - webots_home = get_webots_home() - if webots_home is None: - print('Can\'t load Webots Python API, because Webots is not found.', file=sys.stderr) - return False - if sys.platform == 'linux': - os.environ['LD_LIBRARY_PATH'] = os.path.join(webots_home, 'lib', 'controller') + ':' + os.environ.get('LD_LIBRARY_PATH') - return True - elif sys.platform == 'darwin': - os.environ['DYLD_LIBRARY_PATH'] = os.path.join( - webots_home, 'lib', 'controller') + ':' + os.environ.get('DYLD_LIBRARY_PATH') - return True - elif sys.platform == 'win32': - os.environ['PATH'] = os.path.join(webots_home, 'lib', 'controller') + ';' + os.environ.get('PATH') - return True - else: - print('Unsupported Platform!', file=sys.stderr) - return False - - -def append_webots_python_lib_to_path(): - """Add the Webots 'lib/pythonXY' folder to sys.path.""" - if 'WEBOTS_HOME' not in os.environ: - print('Can\'t load Webots Python API, because Webots is not found.', file=sys.stderr) - return False - - if sys.platform == 'darwin': - sys.path.append( - os.path.join( - os.environ['WEBOTS_HOME'], - 'lib', - 'controller', - 'python%d%d_brew' % (sys.version_info[0], sys.version_info[1]) - ) - ) - return True - elif 'WEBOTS_HOME' in os.environ: - sys.path.append( - os.path.join( - os.environ['WEBOTS_HOME'], - 'lib', - 'controller', - 'python%d%d' % (sys.version_info[0], sys.version_info[1]) - ) - ) - return True - else: - print('No Webots installation has been found!', file=sys.stderr) - return False - - -def __get_archive_name(version): - if sys.platform == 'darwin' or sys.platform == 'linux': - return f'webots-{version.short()}-x86-64.tar.bz2' - return f'webots-{version.short()}_setup.exe' - - -def __install_webots(installation_directory): - target_version = WebotsVersion.target() - - def on_download_progress_changed(count, block_size, total_size): - percent = count*block_size*100/total_size - sys.stdout.write(f'\rDownloading... {percent:.2f}%') - sys.stdout.flush() - - print(f'Installing Webots {target_version}... This might take some time.') - - # Remove previous archive - installation_path = os.path.abspath(os.path.join(installation_directory, 'webots')) - archive_name = __get_archive_name(target_version) - archive_path = os.path.join(installation_directory, archive_name) - - # Remove previous webots folder - if os.path.exists(installation_path): - shutil.rmtree(installation_path) - - # Get Webots archive - if not os.path.exists(archive_path): - url = f'https://github.com/cyberbotics/webots/releases/download/{target_version.short()}/' - urllib.request.urlretrieve(url + archive_name, archive_path, reporthook=on_download_progress_changed) - print('') - else: - print(f'Using installation present at `{archive_path}`...') - - # Extract Webots archive - installation_subdirectory = os.path.join(installation_directory, 'webots' + target_version.short()) - if sys.platform == 'darwin' or sys.platform == 'linux': - print('Extracting...') - tar = tarfile.open(archive_path, 'r:bz2') - tar.extractall(installation_subdirectory) - tar.close() - os.remove(archive_path) - os.environ['WEBOTS_HOME'] = os.path.join(installation_path, 'webots') - else: - print('Installing...') - subprocess.check_output(f'{archive_path} /SILENT /CURRENTUSER', shell=True) - - -def handle_webots_installation(): - target_version = WebotsVersion.target() - installation_directory = os.path.join(str(Path.home()), '.ros') - webots_release_url = f'https://github.com/cyberbotics/webots/releases/tag/{target_version.short()}' - - print( - f'Webots {target_version} was not found in your system.\n' - f'- If you want to manually install Webots {target_version} please download ' - f'it from {webots_release_url}.\n' - f'- If you already have installed Webots {target_version} installed please specify the ' - f'`WEBOTS_HOME` environment variable.\n' - ) - - location_text = '' if sys.platform == 'win32' else f'in `{installation_directory}` ' - method = input( - f'Do you want Webots {target_version} to be automatically installed {location_text}([Y]es/[N]o)?: ') - - if method.lower() == 'y': - __install_webots(installation_directory) - webots_path = get_webots_home() - if webots_path is None: - sys.exit(f'Failed to install Webots {target_version}') - else: - sys.exit(f'Missing Webots version {target_version}') - - -def get_node_name_from_args(): - parser = argparse.ArgumentParser() - parser.add_argument('--webots-node-name', dest='webots_node_name', default='webots_driver', help='Name of your drive node') - args, _ = parser.parse_known_args() - return args.webots_node_name - - -class ControllerLauncher(Node): - """Utility class to launch a controller from a launch file.""" - - def execute(self, context: LaunchContext) -> Optional[List[Action]]: - append_webots_lib_to_path() - return super().execute(context) diff --git a/webots_ros2_core/webots_ros2_core/webots/__init__.py b/webots_ros2_core/webots_ros2_core/webots/__init__.py deleted file mode 100644 index 0054174d7..000000000 --- a/webots_ros2_core/webots_ros2_core/webots/__init__.py +++ /dev/null @@ -1,13 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. diff --git a/webots_ros2_core/webots_ros2_core/webots/controller.py b/webots_ros2_core/webots_ros2_core/webots/controller.py deleted file mode 100644 index 5c727ab84..000000000 --- a/webots_ros2_core/webots_ros2_core/webots/controller.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import sys -from webots_ros2_core.utils import append_webots_python_lib_to_path - -try: - append_webots_python_lib_to_path() -except Exception as e: - sys.stderr.write('"WEBOTS_HOME" is not correctly set.') - raise e - -# deepcode ignore W0401,C0413: This module is just a proxy to `controller` -from controller import * # noqa: F401,F403 diff --git a/webots_ros2_core/webots_ros2_core/webots/vehicle.py b/webots_ros2_core/webots_ros2_core/webots/vehicle.py deleted file mode 100644 index 90918f0cd..000000000 --- a/webots_ros2_core/webots_ros2_core/webots/vehicle.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import sys -from webots_ros2_core.utils import append_webots_python_lib_to_path - -try: - append_webots_python_lib_to_path() -except Exception as e: - sys.stderr.write('"WEBOTS_HOME" is not correctly set.') - raise e - -# deepcode ignore W0401,C0413: This module is just a proxy to `vehicle` -from vehicle import * # noqa: F401,F403 diff --git a/webots_ros2_core/webots_ros2_core/webots_differential_drive_node.py b/webots_ros2_core/webots_ros2_core/webots_differential_drive_node.py deleted file mode 100644 index ed5089277..000000000 --- a/webots_ros2_core/webots_ros2_core/webots_differential_drive_node.py +++ /dev/null @@ -1,234 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import sys - -from math import cos, sin -import rclpy -from rclpy.time import Time -from rcl_interfaces.msg import SetParametersResult -from nav_msgs.msg import Odometry -from geometry_msgs.msg import Twist, TransformStamped -from tf2_ros import TransformBroadcaster -from webots_ros2_core.webots_node import WebotsNode -from webots_ros2_core.utils import get_node_name_from_args - - -class WebotsDifferentialDriveNode(WebotsNode): - """ - Extends WebotsNode to allow easy integration with differential drive robots. - - Args: - ---- - name (WebotsNode): Webots Robot node. - args (dict): Arguments passed to ROS2 base node. - wheel_distance (float): Distance between two wheels (axle length) in meters. - wheel_radius (float): Radius of both wheels in meters. - left_joint (str): Name of motor associated with left wheel. - right_joint (str): Name of motor associated with right wheel. - left_encoder (str): Name of encoder associated with left wheel. - right_encoder (str): Name of encoder associated with right wheel. - command_topic (str): Name of topic to which - [`geometry_msgs/Twist`](https://github.com/ros2/common_interfaces/blob/master/geometry_msgs/msg/Twist.msg) - the node is subscribed to. - odometry_topic (str): Name of topic to which - [`nav_msgs/Odometry`](https://github.com/ros2/common_interfaces/blob/master/nav_msgs/msg/Odometry.msg) - messages are published. - odometry_frame (str): Name of odometry frame. - robot_base_frame (str): Name of robot's base link. - - """ - - def __init__(self, - name, - args, - wheel_distance=0, - wheel_radius=0, - left_joint='left wheel motor', - right_joint='right wheel motor', - left_encoder='left wheel sensor', - right_encoder='right wheel sensor', - command_topic='/cmd_vel', - odometry_topic='/odom', - odometry_frame='odom', - robot_base_frame='base_link' - ): - super().__init__(name, args) - - # Parametrise - wheel_distance_param = self.declare_parameter('wheel_distance', wheel_distance) - wheel_radius_param = self.declare_parameter('wheel_radius', wheel_radius) - left_joint_param = self.declare_parameter('left_joint', left_joint) - right_joint_param = self.declare_parameter('right_joint', right_joint) - left_encoder_param = self.declare_parameter('left_encoder', left_encoder) - right_encoder_param = self.declare_parameter('right_encoder', right_encoder) - command_topic_param = self.declare_parameter('command_topic', command_topic) - odometry_topic_param = self.declare_parameter('odometry_topic', odometry_topic) - odometry_frame_param = self.declare_parameter('odometry_frame', odometry_frame) - robot_base_frame_param = self.declare_parameter('robot_base_frame', robot_base_frame) - self._wheel_radius = wheel_radius_param.value - self._wheel_distance = wheel_distance_param.value - self.set_parameters_callback(self._on_param_changed) - if self._wheel_radius == 0 or self._wheel_distance == 0: - self.get_logger().error('Parameters `wheel_distance` and `wheel_radius` have to greater than 0') - self.destroy_node() - sys.exit(1) - self.get_logger().info( - f'Initializing differential drive node with wheel_distance = {self._wheel_distance} and ' + - f'wheel_radius = {self._wheel_radius}' - ) - - # Store config - self._odometry_frame = odometry_frame_param.value - self._robot_base_frame = robot_base_frame_param.value - - # Initialize motors - self.left_motor = self.robot.getMotor(left_joint_param.value) - self.right_motor = self.robot.getMotor(right_joint_param.value) - self.left_motor.setPosition(float('inf')) - self.right_motor.setPosition(float('inf')) - self.left_motor.setVelocity(0) - self.right_motor.setVelocity(0) - self.create_subscription(Twist, command_topic_param.value, self._cmd_vel_callback, 1) - - # Initialize odometry - self.reset_odometry() - self.left_wheel_sensor = self.robot.getPositionSensor(left_encoder_param.value) - self.right_wheel_sensor = self.robot.getPositionSensor(right_encoder_param.value) - self.left_wheel_sensor.enable(self.timestep) - self.right_wheel_sensor.enable(self.timestep) - self._odometry_publisher = self.create_publisher(Odometry, odometry_topic_param.value, 1) - self._tf_broadcaster = TransformBroadcaster(self) - - # Initialize timer - self._last_odometry_sample_time = self.robot.getTime() - - def _cmd_vel_callback(self, twist): - self.get_logger().info('Message received') - right_velocity = twist.linear.x + self._wheel_distance * twist.angular.z / 2 - left_velocity = twist.linear.x - self._wheel_distance * twist.angular.z / 2 - left_omega = left_velocity / (self._wheel_radius) - right_omega = right_velocity / (self._wheel_radius) - self.left_motor.setVelocity(left_omega) - self.right_motor.setVelocity(right_omega) - - def step(self, ms): - super().step(ms) - - stamp = Time(seconds=self.robot.getTime()).to_msg() - - time_diff_s = self.robot.getTime() - self._last_odometry_sample_time - left_wheel_ticks = self.left_wheel_sensor.getValue() - right_wheel_ticks = self.right_wheel_sensor.getValue() - - if time_diff_s == 0.0: - return - - # Calculate velocities - v_left_rad = (left_wheel_ticks - self._prev_left_wheel_ticks) / time_diff_s - v_right_rad = (right_wheel_ticks - self._prev_right_wheel_ticks) / time_diff_s - v_left = v_left_rad * self._wheel_radius - v_right = v_right_rad * self._wheel_radius - v = (v_left + v_right) / 2 - omega = (v_right - v_left) / self._wheel_distance - - # Calculate position & angle - # Fourth order Runge - Kutta - # Reference: https://www.cs.cmu.edu/~16311/s07/labs/NXTLabs/Lab%203.html - k00 = v * cos(self._prev_angle) - k01 = v * sin(self._prev_angle) - k02 = omega - k10 = v * cos(self._prev_angle + time_diff_s * k02 / 2) - k11 = v * sin(self._prev_angle + time_diff_s * k02 / 2) - k12 = omega - k20 = v * cos(self._prev_angle + time_diff_s * k12 / 2) - k21 = v * sin(self._prev_angle + time_diff_s * k12 / 2) - k22 = omega - k30 = v * cos(self._prev_angle + time_diff_s * k22 / 2) - k31 = v * sin(self._prev_angle + time_diff_s * k22 / 2) - k32 = omega - position = [ - self._prev_position[0] + (time_diff_s / 6) * - (k00 + 2 * (k10 + k20) + k30), - self._prev_position[1] + (time_diff_s / 6) * - (k01 + 2 * (k11 + k21) + k31) - ] - angle = self._prev_angle + \ - (time_diff_s / 6) * (k02 + 2 * (k12 + k22) + k32) - - # Update variables - self._prev_position = position.copy() - self._prev_angle = angle - self._prev_left_wheel_ticks = left_wheel_ticks - self._prev_right_wheel_ticks = right_wheel_ticks - self._last_odometry_sample_time = self.robot.getTime() - - # Pack & publish odometry - msg = Odometry() - msg.header.stamp = stamp - msg.header.frame_id = self._odometry_frame - msg.child_frame_id = self._robot_base_frame - msg.twist.twist.linear.x = v - msg.twist.twist.angular.z = omega - msg.pose.pose.position.x = position[0] - msg.pose.pose.position.y = position[1] - msg.pose.pose.orientation.z = sin(angle / 2) - msg.pose.pose.orientation.w = cos(angle / 2) - self._odometry_publisher.publish(msg) - - # Pack & publish transforms - tf = TransformStamped() - tf.header.stamp = stamp - tf.header.frame_id = self._odometry_frame - tf.child_frame_id = self._robot_base_frame - tf.transform.translation.x = position[0] - tf.transform.translation.y = position[1] - tf.transform.translation.z = 0.0 - tf.transform.rotation.z = sin(angle / 2) - tf.transform.rotation.w = cos(angle / 2) - self._tf_broadcaster.sendTransform(tf) - - def _on_param_changed(self, params): - result = SetParametersResult() - result.successful = True - - for param in params: - if param.name == "wheel_radius": - self.reset_odometry() - self._wheel_radius = param.value - elif param.name == "wheel_distance": - self.reset_odometry() - self._wheel_distance = param.value - - return result - - def reset_odometry(self): - self._prev_left_wheel_ticks = 0 - self._prev_right_wheel_ticks = 0 - self._prev_position = (0.0, 0.0) - self._prev_angle = 0.0 - - -def main(args=None): - rclpy.init(args=args) - - webots_robot_name = get_node_name_from_args() - differential_drive = WebotsDifferentialDriveNode(webots_robot_name, args=args) - differential_drive.start_device_manager() - rclpy.spin(differential_drive) - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/webots_ros2_core/webots_ros2_core/webots_launcher.py b/webots_ros2_core/webots_ros2_core/webots_launcher.py deleted file mode 100644 index b2e16d62e..000000000 --- a/webots_ros2_core/webots_ros2_core/webots_launcher.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""This launcher simply start Webots.""" - -import os -import sys -from launch.actions import ExecuteProcess -from launch.substitution import Substitution -from launch.substitutions import TextSubstitution -from webots_ros2_core.utils import get_webots_home, handle_webots_installation - - -class _WebotsCommandSubstitution(Substitution): - def __init__(self, *, world, gui, mode): - self.__gui = gui if isinstance(gui, Substitution) else TextSubstitution(text=str(gui)) - self.__mode = mode if isinstance(mode, Substitution) else TextSubstitution(text=mode) - self.__world = world if isinstance(world, Substitution) else TextSubstitution(text=world) - - def perform(self, context): - webots_path = get_webots_home(show_warning=True) - if webots_path is None: - handle_webots_installation() - webots_path = get_webots_home() - - # Add `webots` executable to command - if sys.platform == 'win32': - webots_path = os.path.join(webots_path, 'msys64', 'mingw64', 'bin') - command = [r'"{}"'.format(os.path.join(webots_path, 'webots'))] - - # Add `world` - command += [r'"{}"'.format(context.perform_substitution(self.__world))] - - # Hide welcome window - command += ['--batch'] - - # Add parameters to hide GUI if needed - if context.perform_substitution(self.__gui).lower() in ['false', '0']: - command += [ - '--stdout', - '--stderr', - '--batch', - '--minimize', - '--no-rendering' - ] - - # Add mode - command.append('--mode=' + context.perform_substitution(self.__mode)) - return ' '.join(command) - - -class WebotsLauncher(ExecuteProcess): - def __init__(self, output='screen', world=None, gui=True, mode='realtime', **kwargs): - command = _WebotsCommandSubstitution( - world=world, - gui=gui, - mode=mode - ) - - super().__init__( - output=output, - cmd=[command], - shell=True, - **kwargs - ) diff --git a/webots_ros2_core/webots_ros2_core/webots_node.py b/webots_ros2_core/webots_ros2_core/webots_node.py deleted file mode 100644 index 3738511fe..000000000 --- a/webots_ros2_core/webots_ros2_core/webots_node.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python - -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Base node class.""" - -import os -import sys -import argparse -import rclpy -from rclpy.time import Time -from rclpy.node import Node -from rosgraph_msgs.msg import Clock -from webots_ros2_msgs.srv import SetInt -from webots_ros2_core.joint_state_publisher import JointStatePublisher -from webots_ros2_core.devices.device_manager import DeviceManager -from webots_ros2_core.utils import get_node_name_from_args -from webots_ros2_core.webots.controller import Supervisor -from webots_ros2_core.webots.vehicle import Driver - - -MAX_REALTIME_FACTOR = 20 - - -class WebotsNode(Node): - """ - Extends ROS2 base node to provide integration with Webots. - - Args: - ---- - name (str): Webots Robot node. - args (dict): Arguments passed to ROS2 base node. - - """ - - def __init__(self, name, args=None, controller_class=Supervisor): - super().__init__(name) - self.declare_parameter('synchronization', False) - self.declare_parameter('use_joint_state_publisher', False) - parser = argparse.ArgumentParser() - parser.add_argument( - '--webots-robot-name', - dest='webots_robot_name', - default='', - help='Specifies the "name" field of the robot in Webots.' - ) - - # Get robot name - arguments, _ = parser.parse_known_args() - if arguments.webots_robot_name: - os.environ['WEBOTS_CONTROLLER_URL'] = arguments.webots_robot_name - - self.robot = controller_class() - self.timestep = int(self.robot.getBasicTimeStep()) - self.__clock_publisher = self.create_publisher(Clock, 'clock', 10) - self.__step_service = self.create_service(SetInt, 'step', self.__step_callback) - self.__timer = self.create_timer((1 / MAX_REALTIME_FACTOR) * 1e-3 * self.timestep, self.__timer_callback) - self.__device_manager = None - - # Joint state publisher - self.__joint_state_publisher = None - if self.get_parameter('use_joint_state_publisher').value: - self.__joint_state_publisher = JointStatePublisher(self.robot, '', self) - - def start_joint_state_publisher(self): - """Use `JointStatePublisher` to publish ROS2 messages of type `sensor_msgs/JointState`.""" - self.__joint_state_publisher = JointStatePublisher(self.robot, '', self) - - def step(self, ms): - """Call this method on each step.""" - if self.__joint_state_publisher: - self.__joint_state_publisher.publish() - if self.__device_manager: - self.__device_manager.step() - if self.robot is None or self.get_parameter('synchronization').value: - return - - # Robot step - step_result = None - if isinstance(self.robot, Driver): - step_result = self.robot.step() - else: - step_result = self.robot.step(ms) - if step_result < 0: - del self.robot - self.robot = None - sys.exit(0) - - # Update time - msg = Clock() - msg.clock = Time(seconds=self.robot.getTime()).to_msg() - self.__clock_publisher.publish(msg) - - def __timer_callback(self): - self.step(self.timestep) - - def __step_callback(self, request, response): - self.step(request.value) - response.success = True - return response - - def start_device_manager(self, config=None): - """ - Start automatic ROSification of Webots devices available in the robot. - - Kwargs: - config (dict): Dictionary of properties in format:: - - { - [device_name]: { - [property_name]: [property_value] - } - } - - """ - self.__device_manager = DeviceManager(self, config) - - -def main(args=None): - rclpy.init(args=args) - - webots_robot_name = get_node_name_from_args() - driver = WebotsNode(webots_robot_name, args=args) - driver.start_device_manager() - rclpy.spin(driver) - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/webots_ros2_core/webots_ros2_core/webots_robotic_arm_node.py b/webots_ros2_core/webots_ros2_core/webots_robotic_arm_node.py deleted file mode 100644 index c00256bf1..000000000 --- a/webots_ros2_core/webots_ros2_core/webots_robotic_arm_node.py +++ /dev/null @@ -1,64 +0,0 @@ -# Copyright 1996-2021 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - - -import rclpy -from webots_ros2_core.webots_node import WebotsNode -from webots_ros2_core.trajectory_follower import TrajectoryFollower -from webots_ros2_core.utils import get_node_name_from_args - - -class WebotsRoboticArmNode(WebotsNode): - """ - Extends WebotsNode to allow easy integration with robotic arms. - - Args: - ---- - name (WebotsNode): Webots Robot node. - args (dict): Arguments passed to ROS2 base node. - prefix (str): Prefix passed to JointStatePublisher. - - """ - - def __init__(self, name, args, prefix='', controller_name=''): - super().__init__(name, args) - self.start_joint_state_publisher() - self.__prefix_param = self.declare_parameter('prefix', prefix) - self.__controller_name_param = self.declare_parameter('controller_name', controller_name) - self.__trajectory_follower = TrajectoryFollower( - self.robot, - self, - joint_prefix=self.__prefix_param.value, - controller_name=self.__controller_name_param.value - ) - - self.get_logger().info('Initializing robotic arm node with prefix = "%s"' % self.__prefix_param.value) - - -def main(args=None): - rclpy.init(args=args) - - webots_robot_name = get_node_name_from_args() - robotic_arm = WebotsRoboticArmNode(webots_robot_name, args=args) - robotic_arm.start_device_manager() - - # Use a MultiThreadedExecutor to enable processing goals concurrently - executor = rclpy.executors.MultiThreadedExecutor() - - rclpy.spin(robotic_arm, executor=executor) - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/webots_ros2_driver/README.md b/webots_ros2_driver/README.md index d15a36d83..3071a64f2 100644 --- a/webots_ros2_driver/README.md +++ b/webots_ros2_driver/README.md @@ -1,12 +1,10 @@ # Implementation of the interface for Webots and ROS 2 -The main functionalities of the package: +The main functionalities of the package: - It automatically creates a ROS 2 interface out of a Webots robot model. - It allows users to configure the ROS 2 interface in URDF files. - It allows users to extend the interface using the pluginlib plugin mechanism. -> This package aims to replace the `webots_ros2_core` package. - Please use the following links for more details: * [Tutorial: Creating a Custom Package](https://github.com/cyberbotics/webots_ros2/wiki/Tutorial-Creating-a-Custom-Package) From 4cea114609456748c14fee2a0ec92610169d2889 Mon Sep 17 00:00:00 2001 From: Yannick Goumaz <61198661+ygoumaz@users.noreply.github.com> Date: Tue, 22 Nov 2022 12:25:48 +0100 Subject: [PATCH 05/61] Add Python API compatibility and libController compilation (#508) * Update from #503 * Update webots submodule * Update python install cmake * Update libcontroller commit * shallow submodule * Cleanup + rename python modules * Add symlinks for libController + cleanup * Convert to C API * Converting ros2control to C API * Update python plugin to load correct libController * Update repo * pull specific branch from webots * update test version for buildfarm * Revert "update test version for buildfarm" This reverts commit bf800175b418865fda5a497e4646224f33b2fcf1. * Add explanation for WEBOTS_HOME * update to commit * replace submodule by commited files * Delete webots * Rename libcontroller to webots * Add changelog * Update changelog version * Remove useless files --- .gitmodules | 3 - webots_ros2/CHANGELOG.rst | 3 + webots_ros2_control/CHANGELOG.rst | 4 + .../webots_ros2_control/Ros2ControlSystem.hpp | 8 +- .../Ros2ControlSystemInterface.hpp | 2 +- webots_ros2_control/src/Ros2Control.cpp | 10 +- webots_ros2_control/src/Ros2ControlSystem.cpp | 29 +- webots_ros2_driver/CHANGELOG.rst | 6 + webots_ros2_driver/CMakeLists.txt | 170 +- .../include/webots_ros2_driver/WebotsNode.hpp | 7 +- .../plugins/Ros2SensorPlugin.hpp | 2 +- .../plugins/dynamic/Ros2IMU.hpp | 12 +- .../plugins/static/Ros2Camera.hpp | 4 +- .../plugins/static/Ros2DistanceSensor.hpp | 4 +- .../plugins/static/Ros2GPS.hpp | 4 +- .../plugins/static/Ros2LED.hpp | 4 +- .../plugins/static/Ros2Lidar.hpp | 4 +- .../plugins/static/Ros2LightSensor.hpp | 4 +- .../plugins/static/Ros2RangeFinder.hpp | 4 +- webots_ros2_driver/src/Driver.cpp | 21 +- webots_ros2_driver/src/PythonPlugin.cpp | 15 +- webots_ros2_driver/src/WebotsNode.cpp | 41 +- .../src/plugins/Ros2SensorPlugin.cpp | 8 +- .../src/plugins/dynamic/Ros2IMU.cpp | 40 +- .../src/plugins/static/Ros2Camera.cpp | 46 +- .../src/plugins/static/Ros2DistanceSensor.cpp | 18 +- .../src/plugins/static/Ros2GPS.cpp | 22 +- .../src/plugins/static/Ros2LED.cpp | 8 +- .../src/plugins/static/Ros2Lidar.cpp | 44 +- .../src/plugins/static/Ros2LightSensor.cpp | 16 +- .../src/plugins/static/Ros2RangeFinder.cpp | 24 +- webots_ros2_driver/webots | 1 - webots_ros2_driver/webots/.gitignore | 73 + webots_ros2_driver/webots/README.md | 4 + .../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 + .../controller/c/webots/contact_point.h | 25 + .../include/controller/c/webots/device.h | 43 + .../include/controller/c/webots/display.h | 72 + .../controller/c/webots/distance_sensor.h | 56 + .../include/controller/c/webots/emitter.h | 46 + .../webots/include/controller/c/webots/gps.h | 49 + .../webots/include/controller/c/webots/gyro.h | 44 + .../controller/c/webots/inertial_unit.h | 44 + .../include/controller/c/webots/joystick.h | 57 + .../include/controller/c/webots/keyboard.h | 61 + .../webots/include/controller/c/webots/led.h | 38 + .../include/controller/c/webots/lidar.h | 66 + .../include/controller/c/webots/lidar_point.h | 32 + .../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 | 120 + .../webots/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 + .../controller/c/webots/position_sensor.h | 47 + .../include/controller/c/webots/radar.h | 53 + .../controller/c/webots/radar_target.h | 31 + .../include/controller/c/webots/radio.h | 75 + .../controller/c/webots/range_finder.h | 50 + .../include/controller/c/webots/receiver.h | 53 + .../controller/c/webots/remote_control.h | 128 + .../include/controller/c/webots/robot.h | 133 + .../webots/include/controller/c/webots/skin.h | 42 + .../include/controller/c/webots/speaker.h | 49 + .../include/controller/c/webots/supervisor.h | 236 + .../controller/c/webots/touch_sensor.h | 50 + .../include/controller/c/webots/types.h | 82 + .../controller/c/webots/utils/ansi_codes.h | 63 + .../controller/c/webots/utils/motion.h | 47 + .../controller/c/webots/utils/string.h | 35 + .../controller/c/webots/utils/system.h | 55 + .../include/controller/c/webots/vehicle/car.h | 87 + .../controller/c/webots/vehicle/driver.h | 97 + .../controller/cpp/webots/Accelerometer.hpp | 34 + .../controller/cpp/webots/Altimeter.hpp | 32 + .../include/controller/cpp/webots/Brake.hpp | 54 + .../include/controller/cpp/webots/Camera.hpp | 67 + .../include/controller/cpp/webots/Compass.hpp | 34 + .../controller/cpp/webots/Connector.hpp | 35 + .../include/controller/cpp/webots/Device.hpp | 42 + .../include/controller/cpp/webots/Display.hpp | 53 + .../controller/cpp/webots/DistanceSensor.hpp | 40 + .../include/controller/cpp/webots/Emitter.hpp | 35 + .../include/controller/cpp/webots/Field.hpp | 127 + .../include/controller/cpp/webots/GPS.hpp | 42 + .../include/controller/cpp/webots/Gyro.hpp | 34 + .../controller/cpp/webots/ImageRef.hpp | 34 + .../controller/cpp/webots/InertialUnit.hpp | 34 + .../controller/cpp/webots/Joystick.hpp | 43 + .../controller/cpp/webots/Keyboard.hpp | 51 + .../include/controller/cpp/webots/LED.hpp | 30 + .../include/controller/cpp/webots/Lidar.hpp | 51 + .../controller/cpp/webots/LightSensor.hpp | 34 + .../include/controller/cpp/webots/Motor.hpp | 83 + .../include/controller/cpp/webots/Mouse.hpp | 37 + .../include/controller/cpp/webots/Node.hpp | 190 + .../include/controller/cpp/webots/Pen.hpp | 30 + .../controller/cpp/webots/PositionSensor.hpp | 56 + .../include/controller/cpp/webots/Radar.hpp | 40 + .../controller/cpp/webots/RangeFinder.hpp | 38 + .../controller/cpp/webots/Receiver.hpp | 40 + .../include/controller/cpp/webots/Robot.hpp | 176 + .../include/controller/cpp/webots/Skin.hpp | 34 + .../include/controller/cpp/webots/Speaker.hpp | 38 + .../controller/cpp/webots/Supervisor.hpp | 90 + .../controller/cpp/webots/TouchSensor.hpp | 42 + .../controller/cpp/webots/utils/AnsiCodes.hpp | 47 + .../controller/cpp/webots/utils/Motion.hpp | 45 + .../controller/cpp/webots/vehicle/Car.hpp | 65 + .../controller/cpp/webots/vehicle/Driver.hpp | 92 + .../webots/include/plugins/physics.h | 76 + .../webots/include/plugins/radio.h | 82 + .../webots/lib/controller/.gitignore | 13 + .../webots/lib/controller/matlab/.gitignore | 535 + .../webots/lib/controller/matlab/WB_ANGULAR.m | 2 + .../WB_SUPERVISOR_MOVIE_ENCODING_ERROR.m | 4 + .../matlab/WB_SUPERVISOR_MOVIE_READY.m | 4 + .../matlab/WB_SUPERVISOR_MOVIE_RECORDING.m | 4 + .../matlab/WB_SUPERVISOR_MOVIE_SAVING.m | 4 + .../WB_SUPERVISOR_MOVIE_SIMULATION_ERROR.m | 4 + .../matlab/WB_SUPERVISOR_MOVIE_WRITE_ERROR.m | 4 + .../lib/controller/matlab/allincludes.h | 40 + .../webots/lib/controller/matlab/finish.m | 7 + .../webots/lib/controller/matlab/launcher.m | 192 + .../wb_accelerometer_get_lookup_table.m | 9 + .../matlab/wb_accelerometer_get_values.m | 8 + .../controller/matlab/wb_camera_get_image.m | 11 + .../wb_camera_recognition_get_objects.m | 16 + ...amera_recognition_get_segmentation_image.m | 11 + .../matlab/wb_compass_get_lookup_table.m | 9 + .../controller/matlab/wb_compass_get_values.m | 8 + .../matlab/wb_display_draw_polygon.m | 9 + .../matlab/wb_display_fill_polygon.m | 9 + .../controller/matlab/wb_display_image_new.m | 12 + .../controller/matlab/wb_display_set_color.m | 8 + .../wb_distance_sensor_get_lookup_table.m | 9 + .../lib/controller/matlab/wb_emitter_send.m | 7 + .../matlab/wb_gps_get_speed_vector.m | 8 + .../lib/controller/matlab/wb_gps_get_values.m | 8 + .../matlab/wb_gyro_get_lookup_table.m | 9 + .../controller/matlab/wb_gyro_get_values.m | 8 + .../matlab/wb_inertial_unit_get_quaternion.m | 8 + .../wb_inertial_unit_get_roll_pitch_yaw.m | 8 + .../matlab/wb_lidar_get_layer_point_cloud.m | 10 + .../matlab/wb_lidar_get_layer_range_image.m | 9 + .../matlab/wb_lidar_get_point_cloud.m | 21 + .../matlab/wb_lidar_get_range_image.m | 10 + .../matlab/wb_light_sensor_get_lookup_table.m | 9 + .../controller/matlab/wb_mouse_get_state.m | 7 + .../controller/matlab/wb_pen_set_ink_color.m | 8 + .../controller/matlab/wb_radar_get_targets.m | 16 + .../matlab/wb_range_finder_get_range_image.m | 10 + .../matlab/wb_range_finder_image_get_depth.m | 6 + .../controller/matlab/wb_receiver_get_data.m | 22 + .../wb_receiver_get_emitter_direction.m | 8 + .../lib/controller/matlab/wb_robot_cleanup.m | 4 + .../lib/controller/matlab/wb_robot_init.m | 4 + .../matlab/wb_skin_get_bone_orientation.m | 7 + .../matlab/wb_skin_get_bone_position.m | 7 + .../matlab/wb_skin_set_bone_orientation.m | 6 + .../matlab/wb_skin_set_bone_position.m | 6 + .../matlab/wb_supervisor_field_get_mf_color.m | 8 + .../wb_supervisor_field_get_mf_rotation.m | 8 + .../matlab/wb_supervisor_field_get_mf_vec2f.m | 8 + .../matlab/wb_supervisor_field_get_mf_vec3f.m | 8 + .../matlab/wb_supervisor_field_get_sf_color.m | 8 + .../wb_supervisor_field_get_sf_rotation.m | 8 + .../matlab/wb_supervisor_field_get_sf_vec2f.m | 8 + .../matlab/wb_supervisor_field_get_sf_vec3f.m | 8 + .../wb_supervisor_field_insert_mf_color.m | 7 + .../wb_supervisor_field_insert_mf_rotation.m | 7 + .../wb_supervisor_field_insert_mf_vec2f.m | 7 + .../wb_supervisor_field_insert_mf_vec3f.m | 7 + .../matlab/wb_supervisor_field_set_mf_color.m | 7 + .../wb_supervisor_field_set_mf_rotation.m | 7 + .../matlab/wb_supervisor_field_set_mf_vec2f.m | 7 + .../matlab/wb_supervisor_field_set_mf_vec3f.m | 7 + .../matlab/wb_supervisor_field_set_sf_color.m | 7 + .../wb_supervisor_field_set_sf_rotation.m | 7 + .../matlab/wb_supervisor_field_set_sf_vec2f.m | 7 + .../matlab/wb_supervisor_field_set_sf_vec3f.m | 7 + .../matlab/wb_supervisor_get_movie_status.m | 10 + .../matlab/wb_supervisor_load_world.m | 6 + .../matlab/wb_supervisor_movie_get_status.m | 9 + .../wb_supervisor_node_get_center_of_mass.m | 8 + .../wb_supervisor_node_get_contact_point.m | 8 + .../wb_supervisor_node_get_contact_points.m | 19 + .../wb_supervisor_node_get_orientation.m | 8 + .../matlab/wb_supervisor_node_get_pose.m | 13 + .../matlab/wb_supervisor_node_get_position.m | 8 + .../matlab/wb_supervisor_node_get_velocity.m | 8 + .../matlab/wb_supervisor_save_world.m | 11 + .../matlab/wb_supervisor_set_label.m | 8 + .../wb_supervisor_simulation_physics_reset.m | 9 + .../matlab/wb_supervisor_simulation_revert.m | 6 + .../matlab/wb_supervisor_start_movie.m | 10 + .../matlab/wb_supervisor_stop_movie.m | 10 + ..._virtual_reality_headset_get_orientation.m | 8 + ...sor_virtual_reality_headset_get_position.m | 8 + .../matlab/wb_supervisor_world_save.m | 11 + .../matlab/wb_touch_sensor_get_lookup_table.m | 9 + .../matlab/wb_touch_sensor_get_values.m | 8 + .../controller/python/controller/__init__.py | 47 + .../python/controller/accelerometer.py | 43 + .../controller/python/controller/altimeter.py | 34 + .../python/controller/ansi_codes.py | 38 + .../lib/controller/python/controller/brake.py | 57 + .../controller/python/controller/camera.py | 269 + .../controller/python/controller/compass.py | 43 + .../controller/python/controller/connector.py | 55 + .../controller/python/controller/constants.py | 23 + .../controller/python/controller/device.py | 46 + .../controller/python/controller/display.py | 127 + .../python/controller/distance_sensor.py | 77 + .../controller/python/controller/emitter.py | 90 + .../lib/controller/python/controller/field.py | 301 + .../lib/controller/python/controller/gps.py | 68 + .../lib/controller/python/controller/gyro.py | 43 + .../python/controller/inertial_unit.py | 50 + .../controller/python/controller/joystick.py | 97 + .../controller/python/controller/keyboard.py | 110 + .../lib/controller/python/controller/led.py | 39 + .../lib/controller/python/controller/lidar.py | 156 + .../python/controller/lidar_point.py | 25 + .../python/controller/light_sensor.py | 42 + .../controller/python/controller/motion.py | 65 + .../lib/controller/python/controller/motor.py | 238 + .../lib/controller/python/controller/mouse.py | 71 + .../lib/controller/python/controller/node.py | 327 + .../lib/controller/python/controller/pen.py | 29 + .../python/controller/position_sensor.py | 63 + .../lib/controller/python/controller/radar.py | 75 + .../python/controller/radar_target.py | 21 + .../python/controller/range_finder.py | 90 + .../controller/python/controller/receiver.py | 103 + .../lib/controller/python/controller/robot.py | 395 + .../controller/python/controller/sensor.py | 43 + .../lib/controller/python/controller/skin.py | 53 + .../controller/python/controller/speaker.py | 59 + .../python/controller/supervisor.py | 109 + .../python/controller/touch_sensor.py | 61 + .../lib/controller/python/controller/wb.py | 26 + .../lib/controller/python/vehicle/__init__.py | 16 + .../lib/controller/python/vehicle/car.py | 178 + .../lib/controller/python/vehicle/driver.py | 246 + .../default/libraries/vehicle/Makefile | 42 + .../default/libraries/vehicle/c/car/Makefile | 39 + .../default/libraries/vehicle/c/car/car.def | 28 + .../default/libraries/vehicle/c/car/src/car.c | 469 + .../libraries/vehicle/c/car/src/car_private.h | 93 + .../libraries/vehicle/c/driver/Makefile | 39 + .../libraries/vehicle/c/driver/driver.def | 30 + .../libraries/vehicle/c/driver/src/driver.c | 934 + .../libraries/vehicle/cpp/car/Makefile | 39 + .../libraries/vehicle/cpp/car/src/Car.cpp | 121 + .../libraries/vehicle/cpp/driver/Makefile | 40 + .../vehicle/cpp/driver/src/Driver.cpp | 195 + .../default/libraries/vehicle/java/.gitignore | 4 + .../default/libraries/vehicle/java/Makefile | 114 + .../default/libraries/vehicle/java/vehicle.i | 29 + .../libraries/vehicle/python/vehicle310.cpp | 5255 ++++++ .../libraries/vehicle/python/vehicle37.cpp | 5255 ++++++ .../libraries/vehicle/python/vehicle38.cpp | 5255 ++++++ .../libraries/vehicle/python/vehicle39.cpp | 5255 ++++++ .../webots/resources/Makefile.include | 670 + .../webots/resources/Makefile.os.include | 110 + .../libraries/generic_robot_window/Makefile | 26 + .../libraries/generic_robot_window/generic.c | 280 + .../webots/src/controller/Makefile | 34 + .../webots/src/controller/c/Controller.def | 624 + .../webots/src/controller/c/Makefile | 189 + .../webots/src/controller/c/README.TXT | 5 + .../webots/src/controller/c/abstract_camera.c | 171 + .../webots/src/controller/c/abstract_camera.h | 64 + .../webots/src/controller/c/accelerometer.c | 191 + .../webots/src/controller/c/altimeter.c | 144 + .../webots/src/controller/c/ansi_codes.c | 28 + .../webots/src/controller/c/base64.c | 59 + .../webots/src/controller/c/base64.h | 22 + .../webots/src/controller/c/brake.c | 164 + .../webots/src/controller/c/camera.c | 893 + .../webots/src/controller/c/camera_private.h | 48 + .../webots/src/controller/c/compass.c | 190 + .../webots/src/controller/c/connector.c | 187 + .../webots/src/controller/c/console.c | 28 + .../src/controller/c/default_robot_window.c | 871 + .../c/default_robot_window_private.h | 22 + .../webots/src/controller/c/device.c | 167 + .../webots/src/controller/c/device_private.h | 43 + .../webots/src/controller/c/display.c | 887 + .../webots/src/controller/c/display_private.h | 22 + .../webots/src/controller/c/distance_sensor.c | 247 + .../webots/src/controller/c/dynamic_library.c | 86 + .../webots/src/controller/c/dynamic_library.h | 37 + .../webots/src/controller/c/emitter.c | 340 + .../webots/src/controller/c/file.c | 27 + .../webots/src/controller/c/file.h | 25 + .../webots/src/controller/c/g_image.c | 333 + .../webots/src/controller/c/g_image.h | 62 + .../webots/src/controller/c/g_pipe.c | 148 + .../webots/src/controller/c/g_pipe.h | 55 + .../webots/src/controller/c/gps.c | 231 + .../webots/src/controller/c/gyro.c | 191 + .../src/controller/c/html_robot_window.c | 112 + .../controller/c/html_robot_window_private.h | 25 + .../webots/src/controller/c/image.c | 74 + .../webots/src/controller/c/image_private.h | 39 + .../webots/src/controller/c/inertial_unit.c | 195 + .../webots/src/controller/c/joystick.c | 352 + .../src/controller/c/joystick_private.h | 28 + .../webots/src/controller/c/keyboard.c | 128 + .../src/controller/c/keyboard_private.h | 28 + .../webots/src/controller/c/led.c | 102 + .../webots/src/controller/c/lidar.c | 516 + .../webots/src/controller/c/light_sensor.c | 186 + .../webots/src/controller/c/messages.h | 304 + .../webots/src/controller/c/microphone.c | 181 + .../webots/src/controller/c/motion.c | 560 + .../webots/src/controller/c/motion_private.h | 23 + .../webots/src/controller/c/motor.c | 673 + .../webots/src/controller/c/mouse.c | 135 + .../webots/src/controller/c/mouse_private.h | 27 + .../webots/src/controller/c/node.c | 236 + .../webots/src/controller/c/pen.c | 120 + .../webots/src/controller/c/percent.c | 53 + .../webots/src/controller/c/percent.h | 22 + .../webots/src/controller/c/position_sensor.c | 202 + .../webots/src/controller/c/radar.c | 253 + .../webots/src/controller/c/radio.c | 476 + .../webots/src/controller/c/range_finder.c | 335 + .../webots/src/controller/c/receiver.c | 381 + .../webots/src/controller/c/remote_control.c | 486 + .../src/controller/c/remote_control_private.h | 41 + .../webots/src/controller/c/request.c | 223 + .../webots/src/controller/c/request.h | 74 + .../webots/src/controller/c/robot.c | 1509 ++ .../webots/src/controller/c/robot_private.h | 60 + .../webots/src/controller/c/robot_window.c | 166 + .../src/controller/c/robot_window_private.h | 32 + .../webots/src/controller/c/scheduler.c | 367 + .../webots/src/controller/c/scheduler.h | 45 + .../webots/src/controller/c/skin.c | 308 + .../webots/src/controller/c/speaker.c | 563 + .../webots/src/controller/c/string.c | 80 + .../webots/src/controller/c/supervisor.c | 3648 ++++ .../src/controller/c/supervisor_private.h | 24 + .../webots/src/controller/c/system.c | 143 + .../webots/src/controller/c/tcp_client.c | 114 + .../webots/src/controller/c/tcp_client.h | 41 + .../webots/src/controller/c/touch_sensor.c | 251 + .../webots/src/controller/cpp/.gitignore | 2 + .../src/controller/cpp/Accelerometer.cpp | 43 + .../webots/src/controller/cpp/Altimeter.cpp | 38 + .../webots/src/controller/cpp/Brake.cpp | 51 + .../webots/src/controller/cpp/Camera.cpp | 164 + .../webots/src/controller/cpp/Compass.cpp | 43 + .../webots/src/controller/cpp/Connector.cpp | 47 + .../webots/src/controller/cpp/Device.cpp | 41 + .../webots/src/controller/cpp/Display.cpp | 115 + .../src/controller/cpp/DistanceSensor.cpp | 59 + .../webots/src/controller/cpp/Emitter.cpp | 43 + .../webots/src/controller/cpp/Field.cpp | 259 + .../webots/src/controller/cpp/GPS.cpp | 57 + .../webots/src/controller/cpp/Gyro.cpp | 43 + .../src/controller/cpp/InertialUnit.cpp | 43 + .../webots/src/controller/cpp/Joystick.cpp | 80 + .../webots/src/controller/cpp/Keyboard.cpp | 35 + .../webots/src/controller/cpp/LED.cpp | 27 + .../webots/src/controller/cpp/Lidar.cpp | 104 + .../webots/src/controller/cpp/LightSensor.cpp | 43 + .../webots/src/controller/cpp/Makefile | 173 + .../webots/src/controller/cpp/Motion.cpp | 64 + .../webots/src/controller/cpp/Motor.cpp | 156 + .../webots/src/controller/cpp/Mouse.cpp | 47 + .../webots/src/controller/cpp/Node.cpp | 240 + .../webots/src/controller/cpp/Pen.cpp | 27 + .../src/controller/cpp/PositionSensor.cpp | 63 + .../webots/src/controller/cpp/Radar.cpp | 55 + .../webots/src/controller/cpp/RangeFinder.cpp | 64 + .../webots/src/controller/cpp/Receiver.cpp | 63 + .../webots/src/controller/cpp/Robot.cpp | 608 + .../webots/src/controller/cpp/Skin.cpp | 46 + .../webots/src/controller/cpp/Speaker.cpp | 56 + .../webots/src/controller/cpp/Supervisor.cpp | 198 + .../webots/src/controller/cpp/TouchSensor.cpp | 51 + .../webots/src/controller/java/.gitignore | 1 + .../webots/src/controller/java/Makefile | 205 + .../webots/src/controller/java/controller.i | 1263 ++ .../webots/src/controller/matlab/Makefile | 35 + .../webots/src/controller/matlab/mgenerate.py | 726 + webots_ros2_driver/webots/src/stb/.travis.yml | 5 + webots_ros2_driver/webots/src/stb/README.md | 169 + .../src/stb/data/atari_8bit_font_revised.png | Bin 0 -> 1769 bytes .../webots/src/stb/data/easy_font_raw.png | Bin 0 -> 645 bytes .../src/stb/data/herringbone/license.txt | 4 + .../template_caves_limit_connectivity.png | Bin 0 -> 10818 bytes .../template_caves_tiny_corridors.png | Bin 0 -> 4904 bytes .../herringbone/template_corner_caves.png | Bin 0 -> 9337 bytes .../template_horizontal_corridors_v1.png | Bin 0 -> 5491 bytes .../template_horizontal_corridors_v2.png | Bin 0 -> 5759 bytes .../template_horizontal_corridors_v3.png | Bin 0 -> 6766 bytes .../template_limit_connectivity_fat.png | Bin 0 -> 6655 bytes .../template_limited_connectivity.png | Bin 0 -> 6728 bytes .../data/herringbone/template_maze_2_wide.png | Bin 0 -> 12776 bytes .../herringbone/template_maze_plus_2_wide.png | Bin 0 -> 13141 bytes .../data/herringbone/template_open_areas.png | Bin 0 -> 7764 bytes .../template_ref2_corner_caves.png | Bin 0 -> 9729 bytes .../template_rooms_and_corridors.png | Bin 0 -> 4736 bytes ...oms_and_corridors_2_wide_diagonal_bias.png | Bin 0 -> 4193 bytes .../template_rooms_limit_connectivity.png | Bin 0 -> 7664 bytes ...emplate_round_rooms_diagonal_corridors.png | Bin 0 -> 8172 bytes .../herringbone/template_sean_dungeon.png | Bin 0 -> 10261 bytes .../template_simple_caves_2_wide.png | Bin 0 -> 15712 bytes ...emplate_square_rooms_with_random_rects.png | Bin 0 -> 4772 bytes .../webots/src/stb/data/map_01.png | Bin 0 -> 30625 bytes .../webots/src/stb/data/map_02.png | Bin 0 -> 3243 bytes .../webots/src/stb/data/map_03.png | Bin 0 -> 9620 bytes .../webots/src/stb/deprecated/rrsprintf.h | 1055 ++ .../webots/src/stb/deprecated/stb_image.c | 4678 +++++ .../src/stb/deprecated/stretchy_buffer.txt | 28 + .../webots/src/stb/docs/other_libs.md | 1 + .../webots/src/stb/docs/stb_howto.txt | 185 + .../stb/docs/stb_voxel_render_interview.md | 173 + .../webots/src/stb/docs/why_public_domain.md | 116 + webots_ros2_driver/webots/src/stb/stb.h | 14453 ++++++++++++++++ .../webots/src/stb/stb_c_lexer.h | 966 ++ .../webots/src/stb/stb_connected_components.h | 1049 ++ .../webots/src/stb/stb_divide.h | 421 + webots_ros2_driver/webots/src/stb/stb_ds.h | 1704 ++ webots_ros2_driver/webots/src/stb/stb_dxt.h | 728 + .../webots/src/stb/stb_easy_font.h | 303 + .../src/stb/stb_herringbone_wang_tile.h | 1221 ++ webots_ros2_driver/webots/src/stb/stb_image.h | 7559 ++++++++ .../webots/src/stb/stb_image_resize.h | 2630 +++ .../webots/src/stb/stb_image_write.h | 1619 ++ .../webots/src/stb/stb_include.h | 288 + .../webots/src/stb/stb_leakcheck.h | 190 + .../webots/src/stb/stb_perlin.h | 427 + .../webots/src/stb/stb_rect_pack.h | 628 + .../webots/src/stb/stb_sprintf.h | 1860 ++ .../webots/src/stb/stb_textedit.h | 1404 ++ .../webots/src/stb/stb_tilemap_editor.h | 4161 +++++ .../webots/src/stb/stb_truetype.h | 4888 ++++++ .../webots/src/stb/stb_vorbis.c | 5502 ++++++ .../webots/src/stb/stb_voxel_render.h | 3806 ++++ .../webots/src/stb/stretchy_buffer.h | 262 + .../webots/src/stb/tests/Makefile | 10 + .../webots/src/stb/tests/c_lexer_test.c | 50 + .../webots/src/stb/tests/c_lexer_test.dsp | 89 + .../webots/src/stb/tests/caveview/README.md | 85 + .../webots/src/stb/tests/caveview/cave_main.c | 598 + .../src/stb/tests/caveview/cave_mesher.c | 933 + .../src/stb/tests/caveview/cave_parse.c | 632 + .../src/stb/tests/caveview/cave_parse.h | 41 + .../src/stb/tests/caveview/cave_render.c | 951 + .../src/stb/tests/caveview/caveview.dsp | 157 + .../src/stb/tests/caveview/caveview.dsw | 29 + .../webots/src/stb/tests/caveview/caveview.h | 50 + .../webots/src/stb/tests/caveview/glext.h | 11124 ++++++++++++ .../src/stb/tests/caveview/glext_list.h | 34 + .../webots/src/stb/tests/caveview/main.c | 0 .../webots/src/stb/tests/caveview/stb_gl.h | 1103 ++ .../src/stb/tests/caveview/stb_glprog.h | 504 + .../tests/caveview/win32/SDL_windows_main.c | 224 + .../webots/src/stb/tests/grid_reachability.c | 363 + .../webots/src/stb/tests/herringbone.dsp | 95 + .../src/stb/tests/herringbone_generator.c | 87 + .../webots/src/stb/tests/herringbone_map.c | 83 + .../webots/src/stb/tests/herringbone_map.dsp | 94 + .../webots/src/stb/tests/image_test.c | 173 + .../webots/src/stb/tests/image_test.dsp | 102 + .../webots/src/stb/tests/image_write_test.c | 60 + .../webots/src/stb/tests/oversample/README.md | 94 + .../webots/src/stb/tests/oversample/main.c | 332 + .../src/stb/tests/oversample/oversample.dsp | 97 + .../src/stb/tests/oversample/oversample.dsw | 29 + .../src/stb/tests/oversample/stb_wingraph.h | 829 + .../webots/src/stb/tests/pg_test/pg_test.c | 124 + .../src/stb/tests/pngsuite/16bit/basi0g16.png | Bin 0 -> 299 bytes .../src/stb/tests/pngsuite/16bit/basi2c16.png | Bin 0 -> 595 bytes .../src/stb/tests/pngsuite/16bit/basi4a16.png | Bin 0 -> 2855 bytes .../src/stb/tests/pngsuite/16bit/basi6a16.png | Bin 0 -> 4180 bytes .../src/stb/tests/pngsuite/16bit/basn0g16.png | Bin 0 -> 167 bytes .../src/stb/tests/pngsuite/16bit/basn2c16.png | Bin 0 -> 302 bytes .../src/stb/tests/pngsuite/16bit/basn4a16.png | Bin 0 -> 2206 bytes .../src/stb/tests/pngsuite/16bit/basn6a16.png | Bin 0 -> 3435 bytes .../src/stb/tests/pngsuite/16bit/bgai4a16.png | Bin 0 -> 2855 bytes .../src/stb/tests/pngsuite/16bit/bgan6a16.png | Bin 0 -> 3435 bytes .../src/stb/tests/pngsuite/16bit/bggn4a16.png | Bin 0 -> 2220 bytes .../src/stb/tests/pngsuite/16bit/bgyn6a16.png | Bin 0 -> 3453 bytes .../src/stb/tests/pngsuite/16bit/oi1n0g16.png | Bin 0 -> 167 bytes .../src/stb/tests/pngsuite/16bit/oi1n2c16.png | Bin 0 -> 302 bytes .../src/stb/tests/pngsuite/16bit/oi2n0g16.png | Bin 0 -> 179 bytes .../src/stb/tests/pngsuite/16bit/oi2n2c16.png | Bin 0 -> 314 bytes .../src/stb/tests/pngsuite/16bit/oi4n0g16.png | Bin 0 -> 203 bytes .../src/stb/tests/pngsuite/16bit/oi4n2c16.png | Bin 0 -> 338 bytes .../src/stb/tests/pngsuite/16bit/oi9n0g16.png | Bin 0 -> 1283 bytes .../src/stb/tests/pngsuite/16bit/oi9n2c16.png | Bin 0 -> 3038 bytes .../src/stb/tests/pngsuite/16bit/tbbn2c16.png | Bin 0 -> 2041 bytes .../src/stb/tests/pngsuite/16bit/tbgn2c16.png | Bin 0 -> 2041 bytes .../src/stb/tests/pngsuite/16bit/tbwn0g16.png | Bin 0 -> 1313 bytes .../src/stb/tests/pngsuite/PngSuite.LICENSE | 9 + .../stb/tests/pngsuite/corrupt/xc1n0g08.png | Bin 0 -> 138 bytes .../stb/tests/pngsuite/corrupt/xc9n2c08.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xcrn0g04.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xcsn0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/corrupt/xd0n2c08.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xd3n2c08.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xd9n2c08.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xdtn0g01.png | Bin 0 -> 61 bytes .../stb/tests/pngsuite/corrupt/xhdn0g08.png | Bin 0 -> 138 bytes .../stb/tests/pngsuite/corrupt/xlfn0g04.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/corrupt/xs1n0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/corrupt/xs2n0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/corrupt/xs4n0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/corrupt/xs7n0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/primary/basi0g01.png | Bin 0 -> 217 bytes .../stb/tests/pngsuite/primary/basi0g02.png | Bin 0 -> 154 bytes .../stb/tests/pngsuite/primary/basi0g04.png | Bin 0 -> 247 bytes .../stb/tests/pngsuite/primary/basi0g08.png | Bin 0 -> 254 bytes .../stb/tests/pngsuite/primary/basi2c08.png | Bin 0 -> 315 bytes .../stb/tests/pngsuite/primary/basi3p01.png | Bin 0 -> 132 bytes .../stb/tests/pngsuite/primary/basi3p02.png | Bin 0 -> 193 bytes .../stb/tests/pngsuite/primary/basi3p04.png | Bin 0 -> 327 bytes .../stb/tests/pngsuite/primary/basi3p08.png | Bin 0 -> 1527 bytes .../stb/tests/pngsuite/primary/basi4a08.png | Bin 0 -> 214 bytes .../stb/tests/pngsuite/primary/basi6a08.png | Bin 0 -> 361 bytes .../stb/tests/pngsuite/primary/basn0g01.png | Bin 0 -> 164 bytes .../stb/tests/pngsuite/primary/basn0g02.png | Bin 0 -> 104 bytes .../stb/tests/pngsuite/primary/basn0g04.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/primary/basn0g08.png | Bin 0 -> 138 bytes .../stb/tests/pngsuite/primary/basn2c08.png | Bin 0 -> 145 bytes .../stb/tests/pngsuite/primary/basn3p01.png | Bin 0 -> 112 bytes .../stb/tests/pngsuite/primary/basn3p02.png | Bin 0 -> 146 bytes .../stb/tests/pngsuite/primary/basn3p04.png | Bin 0 -> 216 bytes .../stb/tests/pngsuite/primary/basn3p08.png | Bin 0 -> 1286 bytes .../stb/tests/pngsuite/primary/basn4a08.png | Bin 0 -> 126 bytes .../stb/tests/pngsuite/primary/basn6a08.png | Bin 0 -> 184 bytes .../stb/tests/pngsuite/primary/bgai4a08.png | Bin 0 -> 214 bytes .../stb/tests/pngsuite/primary/bgan6a08.png | Bin 0 -> 184 bytes .../stb/tests/pngsuite/primary/bgbn4a08.png | Bin 0 -> 140 bytes .../stb/tests/pngsuite/primary/bgwn6a08.png | Bin 0 -> 202 bytes .../stb/tests/pngsuite/primary/s01i3p01.png | Bin 0 -> 113 bytes .../stb/tests/pngsuite/primary/s01n3p01.png | Bin 0 -> 113 bytes .../stb/tests/pngsuite/primary/s02i3p01.png | Bin 0 -> 114 bytes .../stb/tests/pngsuite/primary/s02n3p01.png | Bin 0 -> 115 bytes .../stb/tests/pngsuite/primary/s03i3p01.png | Bin 0 -> 118 bytes .../stb/tests/pngsuite/primary/s03n3p01.png | Bin 0 -> 120 bytes .../stb/tests/pngsuite/primary/s04i3p01.png | Bin 0 -> 126 bytes .../stb/tests/pngsuite/primary/s04n3p01.png | Bin 0 -> 121 bytes .../stb/tests/pngsuite/primary/s05i3p02.png | Bin 0 -> 134 bytes .../stb/tests/pngsuite/primary/s05n3p02.png | Bin 0 -> 129 bytes .../stb/tests/pngsuite/primary/s06i3p02.png | Bin 0 -> 143 bytes .../stb/tests/pngsuite/primary/s06n3p02.png | Bin 0 -> 131 bytes .../stb/tests/pngsuite/primary/s07i3p02.png | Bin 0 -> 149 bytes .../stb/tests/pngsuite/primary/s07n3p02.png | Bin 0 -> 138 bytes .../stb/tests/pngsuite/primary/s08i3p02.png | Bin 0 -> 149 bytes .../stb/tests/pngsuite/primary/s08n3p02.png | Bin 0 -> 139 bytes .../stb/tests/pngsuite/primary/s09i3p02.png | Bin 0 -> 147 bytes .../stb/tests/pngsuite/primary/s09n3p02.png | Bin 0 -> 143 bytes .../stb/tests/pngsuite/primary/s32i3p04.png | Bin 0 -> 355 bytes .../stb/tests/pngsuite/primary/s32n3p04.png | Bin 0 -> 263 bytes .../stb/tests/pngsuite/primary/s33i3p04.png | Bin 0 -> 385 bytes .../stb/tests/pngsuite/primary/s33n3p04.png | Bin 0 -> 329 bytes .../stb/tests/pngsuite/primary/s34i3p04.png | Bin 0 -> 349 bytes .../stb/tests/pngsuite/primary/s34n3p04.png | Bin 0 -> 248 bytes .../stb/tests/pngsuite/primary/s35i3p04.png | Bin 0 -> 399 bytes .../stb/tests/pngsuite/primary/s35n3p04.png | Bin 0 -> 338 bytes .../stb/tests/pngsuite/primary/s36i3p04.png | Bin 0 -> 356 bytes .../stb/tests/pngsuite/primary/s36n3p04.png | Bin 0 -> 258 bytes .../stb/tests/pngsuite/primary/s37i3p04.png | Bin 0 -> 393 bytes .../stb/tests/pngsuite/primary/s37n3p04.png | Bin 0 -> 336 bytes .../stb/tests/pngsuite/primary/s38i3p04.png | Bin 0 -> 357 bytes .../stb/tests/pngsuite/primary/s38n3p04.png | Bin 0 -> 245 bytes .../stb/tests/pngsuite/primary/s39i3p04.png | Bin 0 -> 420 bytes .../stb/tests/pngsuite/primary/s39n3p04.png | Bin 0 -> 352 bytes .../stb/tests/pngsuite/primary/s40i3p04.png | Bin 0 -> 357 bytes .../stb/tests/pngsuite/primary/s40n3p04.png | Bin 0 -> 256 bytes .../stb/tests/pngsuite/primary/tbbn0g04.png | Bin 0 -> 429 bytes .../stb/tests/pngsuite/primary/tbbn3p08.png | Bin 0 -> 1499 bytes .../stb/tests/pngsuite/primary/tbgn3p08.png | Bin 0 -> 1499 bytes .../stb/tests/pngsuite/primary/tbrn2c08.png | Bin 0 -> 1633 bytes .../stb/tests/pngsuite/primary/tbwn3p08.png | Bin 0 -> 1496 bytes .../stb/tests/pngsuite/primary/tbyn3p08.png | Bin 0 -> 1499 bytes .../stb/tests/pngsuite/primary/tm3n3p02.png | Bin 0 -> 116 bytes .../stb/tests/pngsuite/primary/tp0n0g08.png | Bin 0 -> 719 bytes .../stb/tests/pngsuite/primary/tp0n2c08.png | Bin 0 -> 1594 bytes .../stb/tests/pngsuite/primary/tp0n3p08.png | Bin 0 -> 1476 bytes .../stb/tests/pngsuite/primary/tp1n3p08.png | Bin 0 -> 1483 bytes .../stb/tests/pngsuite/primary/z00n2c08.png | Bin 0 -> 3172 bytes .../stb/tests/pngsuite/primary/z03n2c08.png | Bin 0 -> 232 bytes .../stb/tests/pngsuite/primary/z06n2c08.png | Bin 0 -> 224 bytes .../stb/tests/pngsuite/primary/z09n2c08.png | Bin 0 -> 224 bytes .../tests/pngsuite/primary_check/basi0g01.png | Bin 0 -> 391 bytes .../tests/pngsuite/primary_check/basi0g02.png | Bin 0 -> 283 bytes .../tests/pngsuite/primary_check/basi0g04.png | Bin 0 -> 252 bytes .../tests/pngsuite/primary_check/basi0g08.png | Bin 0 -> 293 bytes .../tests/pngsuite/primary_check/basi2c08.png | Bin 0 -> 274 bytes .../tests/pngsuite/primary_check/basi3p01.png | Bin 0 -> 273 bytes .../tests/pngsuite/primary_check/basi3p02.png | Bin 0 -> 286 bytes .../tests/pngsuite/primary_check/basi3p04.png | Bin 0 -> 331 bytes .../tests/pngsuite/primary_check/basi3p08.png | Bin 0 -> 387 bytes .../tests/pngsuite/primary_check/basi4a08.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/basi6a08.png | Bin 0 -> 298 bytes .../tests/pngsuite/primary_check/basn0g01.png | Bin 0 -> 391 bytes .../tests/pngsuite/primary_check/basn0g02.png | Bin 0 -> 283 bytes .../tests/pngsuite/primary_check/basn0g04.png | Bin 0 -> 252 bytes .../tests/pngsuite/primary_check/basn0g08.png | Bin 0 -> 293 bytes .../tests/pngsuite/primary_check/basn2c08.png | Bin 0 -> 274 bytes .../tests/pngsuite/primary_check/basn3p01.png | Bin 0 -> 273 bytes .../tests/pngsuite/primary_check/basn3p02.png | Bin 0 -> 286 bytes .../tests/pngsuite/primary_check/basn3p04.png | Bin 0 -> 331 bytes .../tests/pngsuite/primary_check/basn3p08.png | Bin 0 -> 387 bytes .../tests/pngsuite/primary_check/basn4a08.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/basn6a08.png | Bin 0 -> 298 bytes .../tests/pngsuite/primary_check/bgai4a08.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/bgan6a08.png | Bin 0 -> 298 bytes .../tests/pngsuite/primary_check/bgbn4a08.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/bgwn6a08.png | Bin 0 -> 298 bytes .../tests/pngsuite/primary_check/s01i3p01.png | Bin 0 -> 202 bytes .../tests/pngsuite/primary_check/s01n3p01.png | Bin 0 -> 202 bytes .../tests/pngsuite/primary_check/s02i3p01.png | Bin 0 -> 210 bytes .../tests/pngsuite/primary_check/s02n3p01.png | Bin 0 -> 210 bytes .../tests/pngsuite/primary_check/s03i3p01.png | Bin 0 -> 216 bytes .../tests/pngsuite/primary_check/s03n3p01.png | Bin 0 -> 216 bytes .../tests/pngsuite/primary_check/s04i3p01.png | Bin 0 -> 221 bytes .../tests/pngsuite/primary_check/s04n3p01.png | Bin 0 -> 221 bytes .../tests/pngsuite/primary_check/s05i3p02.png | Bin 0 -> 232 bytes .../tests/pngsuite/primary_check/s05n3p02.png | Bin 0 -> 232 bytes .../tests/pngsuite/primary_check/s06i3p02.png | Bin 0 -> 239 bytes .../tests/pngsuite/primary_check/s06n3p02.png | Bin 0 -> 239 bytes .../tests/pngsuite/primary_check/s07i3p02.png | Bin 0 -> 249 bytes .../tests/pngsuite/primary_check/s07n3p02.png | Bin 0 -> 249 bytes .../tests/pngsuite/primary_check/s08i3p02.png | Bin 0 -> 255 bytes .../tests/pngsuite/primary_check/s08n3p02.png | Bin 0 -> 255 bytes .../tests/pngsuite/primary_check/s09i3p02.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/s09n3p02.png | Bin 0 -> 263 bytes .../tests/pngsuite/primary_check/s32i3p04.png | Bin 0 -> 441 bytes .../tests/pngsuite/primary_check/s32n3p04.png | Bin 0 -> 441 bytes .../tests/pngsuite/primary_check/s33i3p04.png | Bin 0 -> 470 bytes .../tests/pngsuite/primary_check/s33n3p04.png | Bin 0 -> 470 bytes .../tests/pngsuite/primary_check/s34i3p04.png | Bin 0 -> 431 bytes .../tests/pngsuite/primary_check/s34n3p04.png | Bin 0 -> 431 bytes .../tests/pngsuite/primary_check/s35i3p04.png | Bin 0 -> 477 bytes .../tests/pngsuite/primary_check/s35n3p04.png | Bin 0 -> 477 bytes .../tests/pngsuite/primary_check/s36i3p04.png | Bin 0 -> 448 bytes .../tests/pngsuite/primary_check/s36n3p04.png | Bin 0 -> 448 bytes .../tests/pngsuite/primary_check/s37i3p04.png | Bin 0 -> 478 bytes .../tests/pngsuite/primary_check/s37n3p04.png | Bin 0 -> 478 bytes .../tests/pngsuite/primary_check/s38i3p04.png | Bin 0 -> 439 bytes .../tests/pngsuite/primary_check/s38n3p04.png | Bin 0 -> 439 bytes .../tests/pngsuite/primary_check/s39i3p04.png | Bin 0 -> 499 bytes .../tests/pngsuite/primary_check/s39n3p04.png | Bin 0 -> 499 bytes .../tests/pngsuite/primary_check/s40i3p04.png | Bin 0 -> 463 bytes .../tests/pngsuite/primary_check/s40n3p04.png | Bin 0 -> 463 bytes .../tests/pngsuite/primary_check/tbbn0g04.png | Bin 0 -> 762 bytes .../tests/pngsuite/primary_check/tbbn3p08.png | Bin 0 -> 1911 bytes .../tests/pngsuite/primary_check/tbgn3p08.png | Bin 0 -> 1911 bytes .../tests/pngsuite/primary_check/tbrn2c08.png | Bin 0 -> 1901 bytes .../tests/pngsuite/primary_check/tbwn3p08.png | Bin 0 -> 1911 bytes .../tests/pngsuite/primary_check/tbyn3p08.png | Bin 0 -> 1911 bytes .../tests/pngsuite/primary_check/tm3n3p02.png | Bin 0 -> 306 bytes .../tests/pngsuite/primary_check/tp0n0g08.png | Bin 0 -> 1802 bytes .../tests/pngsuite/primary_check/tp0n2c08.png | Bin 0 -> 1955 bytes .../tests/pngsuite/primary_check/tp0n3p08.png | Bin 0 -> 1959 bytes .../tests/pngsuite/primary_check/tp1n3p08.png | Bin 0 -> 1911 bytes .../tests/pngsuite/primary_check/z00n2c08.png | Bin 0 -> 422 bytes .../tests/pngsuite/primary_check/z03n2c08.png | Bin 0 -> 422 bytes .../tests/pngsuite/primary_check/z06n2c08.png | Bin 0 -> 422 bytes .../tests/pngsuite/primary_check/z09n2c08.png | Bin 0 -> 422 bytes .../stb/tests/pngsuite/unused/ccwn2c08.png | Bin 0 -> 1514 bytes .../stb/tests/pngsuite/unused/ccwn3p08.png | Bin 0 -> 1554 bytes .../stb/tests/pngsuite/unused/cdfn2c08.png | Bin 0 -> 404 bytes .../stb/tests/pngsuite/unused/cdhn2c08.png | Bin 0 -> 344 bytes .../stb/tests/pngsuite/unused/cdsn2c08.png | Bin 0 -> 232 bytes .../stb/tests/pngsuite/unused/cdun2c08.png | Bin 0 -> 724 bytes .../stb/tests/pngsuite/unused/ch1n3p04.png | Bin 0 -> 258 bytes .../stb/tests/pngsuite/unused/ch2n3p08.png | Bin 0 -> 1810 bytes .../stb/tests/pngsuite/unused/cm0n0g04.png | Bin 0 -> 292 bytes .../stb/tests/pngsuite/unused/cm7n0g04.png | Bin 0 -> 292 bytes .../stb/tests/pngsuite/unused/cm9n0g04.png | Bin 0 -> 292 bytes .../stb/tests/pngsuite/unused/cs3n2c16.png | Bin 0 -> 214 bytes .../stb/tests/pngsuite/unused/cs3n3p08.png | Bin 0 -> 259 bytes .../stb/tests/pngsuite/unused/cs5n2c08.png | Bin 0 -> 186 bytes .../stb/tests/pngsuite/unused/cs5n3p08.png | Bin 0 -> 271 bytes .../stb/tests/pngsuite/unused/cs8n2c08.png | Bin 0 -> 149 bytes .../stb/tests/pngsuite/unused/cs8n3p08.png | Bin 0 -> 256 bytes .../stb/tests/pngsuite/unused/ct0n0g04.png | Bin 0 -> 273 bytes .../stb/tests/pngsuite/unused/ct1n0g04.png | Bin 0 -> 792 bytes .../stb/tests/pngsuite/unused/cten0g04.png | Bin 0 -> 742 bytes .../stb/tests/pngsuite/unused/ctfn0g04.png | Bin 0 -> 716 bytes .../stb/tests/pngsuite/unused/ctgn0g04.png | Bin 0 -> 1182 bytes .../stb/tests/pngsuite/unused/cthn0g04.png | Bin 0 -> 1269 bytes .../stb/tests/pngsuite/unused/ctjn0g04.png | Bin 0 -> 941 bytes .../stb/tests/pngsuite/unused/ctzn0g04.png | Bin 0 -> 753 bytes .../stb/tests/pngsuite/unused/f00n0g08.png | Bin 0 -> 319 bytes .../stb/tests/pngsuite/unused/f00n2c08.png | Bin 0 -> 2475 bytes .../stb/tests/pngsuite/unused/f01n0g08.png | Bin 0 -> 321 bytes .../stb/tests/pngsuite/unused/f01n2c08.png | Bin 0 -> 1180 bytes .../stb/tests/pngsuite/unused/f02n0g08.png | Bin 0 -> 355 bytes .../stb/tests/pngsuite/unused/f02n2c08.png | Bin 0 -> 1729 bytes .../stb/tests/pngsuite/unused/f03n0g08.png | Bin 0 -> 389 bytes .../stb/tests/pngsuite/unused/f03n2c08.png | Bin 0 -> 1291 bytes .../stb/tests/pngsuite/unused/f04n0g08.png | Bin 0 -> 269 bytes .../stb/tests/pngsuite/unused/f04n2c08.png | Bin 0 -> 985 bytes .../stb/tests/pngsuite/unused/f99n0g04.png | Bin 0 -> 426 bytes .../stb/tests/pngsuite/unused/g03n0g16.png | Bin 0 -> 345 bytes .../stb/tests/pngsuite/unused/g03n2c08.png | Bin 0 -> 370 bytes .../stb/tests/pngsuite/unused/g03n3p04.png | Bin 0 -> 214 bytes .../stb/tests/pngsuite/unused/g04n0g16.png | Bin 0 -> 363 bytes .../stb/tests/pngsuite/unused/g04n2c08.png | Bin 0 -> 377 bytes .../stb/tests/pngsuite/unused/g04n3p04.png | Bin 0 -> 219 bytes .../stb/tests/pngsuite/unused/g05n0g16.png | Bin 0 -> 339 bytes .../stb/tests/pngsuite/unused/g05n2c08.png | Bin 0 -> 350 bytes .../stb/tests/pngsuite/unused/g05n3p04.png | Bin 0 -> 206 bytes .../stb/tests/pngsuite/unused/g07n0g16.png | Bin 0 -> 321 bytes .../stb/tests/pngsuite/unused/g07n2c08.png | Bin 0 -> 340 bytes .../stb/tests/pngsuite/unused/g07n3p04.png | Bin 0 -> 207 bytes .../stb/tests/pngsuite/unused/g10n0g16.png | Bin 0 -> 262 bytes .../stb/tests/pngsuite/unused/g10n2c08.png | Bin 0 -> 285 bytes .../stb/tests/pngsuite/unused/g10n3p04.png | Bin 0 -> 214 bytes .../stb/tests/pngsuite/unused/g25n0g16.png | Bin 0 -> 383 bytes .../stb/tests/pngsuite/unused/g25n2c08.png | Bin 0 -> 405 bytes .../stb/tests/pngsuite/unused/g25n3p04.png | Bin 0 -> 215 bytes .../stb/tests/pngsuite/unused/pp0n2c16.png | Bin 0 -> 962 bytes .../stb/tests/pngsuite/unused/pp0n6a08.png | Bin 0 -> 818 bytes .../stb/tests/pngsuite/unused/ps1n0g08.png | Bin 0 -> 1456 bytes .../stb/tests/pngsuite/unused/ps1n2c16.png | Bin 0 -> 1620 bytes .../stb/tests/pngsuite/unused/ps2n0g08.png | Bin 0 -> 2320 bytes .../stb/tests/pngsuite/unused/ps2n2c16.png | Bin 0 -> 2484 bytes .../webots/src/stb/tests/prerelease/stb_lib.h | 3305 ++++ .../webots/src/stb/tests/resample_test.cpp | 1122 ++ .../webots/src/stb/tests/resample_test_c.c | 8 + .../webots/src/stb/tests/resize.dsp | 94 + .../webots/src/stb/tests/sdf/sdf_test.c | 152 + .../src/stb/tests/sdf/sdf_test_arial_16.png | Bin 0 -> 121269 bytes .../src/stb/tests/sdf/sdf_test_times_16.png | Bin 0 -> 108371 bytes .../src/stb/tests/sdf/sdf_test_times_50.png | Bin 0 -> 104962 bytes webots_ros2_driver/webots/src/stb/tests/stb.c | 3330 ++++ .../webots/src/stb/tests/stb.dsp | 252 + .../webots/src/stb/tests/stb.dsw | 173 + .../webots/src/stb/tests/stb_cpp.cpp | 83 + .../webots/src/stb/tests/stb_cpp.dsp | 98 + .../webots/src/stb/tests/stb_static.c | 12 + .../webots/src/stb/tests/stblib.dsp | 102 + .../webots/src/stb/tests/stblib_test.c | 11 + .../src/stb/tests/stblib_test_companion.c | 4 + .../webots/src/stb/tests/stretch_test.c | 28 + .../webots/src/stb/tests/stretch_test.dsp | 89 + .../src/stb/tests/stretchy_buffer_test.c | 7 + .../webots/src/stb/tests/test.sbm | 60 + .../webots/src/stb/tests/test_c_compilation.c | 42 + .../webots/src/stb/tests/test_c_lexer.c | 1 + .../src/stb/tests/test_cpp_compilation.cpp | 170 + .../webots/src/stb/tests/test_ds.c | 1030 ++ .../webots/src/stb/tests/test_ds_cpp.cpp | 418 + .../webots/src/stb/tests/test_dxt.c | 1 + .../webots/src/stb/tests/test_easyfont.c | 10 + .../webots/src/stb/tests/test_image.c | 7 + .../webots/src/stb/tests/test_image_write.c | 7 + .../webots/src/stb/tests/test_perlin.c | 1 + .../webots/src/stb/tests/test_siphash.c | 21 + .../webots/src/stb/tests/test_sprintf.c | 1 + .../webots/src/stb/tests/test_truetype.c | 105 + .../webots/src/stb/tests/test_vorbis.c | 18 + .../webots/src/stb/tests/test_voxel.c | 1 + .../webots/src/stb/tests/textedit_sample.c | 94 + .../tilemap_editor_integration_example.c | 193 + .../webots/src/stb/tests/vorbseek/vorbseek.c | 125 + .../src/stb/tests/vorbseek/vorbseek.dsp | 96 + .../webots/src/stb/tools/README.footer.md | 121 + .../webots/src/stb/tools/README.header.md | 19 + .../webots/src/stb/tools/README.list | 22 + .../webots/src/stb/tools/build_matrix.c | 132 + .../webots/src/stb/tools/easy_font_maker.c | 211 + .../webots/src/stb/tools/make_readme.c | 64 + .../webots/src/stb/tools/make_readme.dsp | 97 + .../webots/src/stb/tools/mr.bat | 1 + .../webots/src/stb/tools/unicode.c | 749 + .../webots/src/stb/tools/unicode/unicode.dsp | 88 + .../webots_ros2_driver/ros2_supervisor.py | 8 +- .../webots_ros2_driver/webots_launcher.py | 7 +- 794 files changed, 156256 insertions(+), 300 deletions(-) delete mode 160000 webots_ros2_driver/webots create mode 100644 webots_ros2_driver/webots/.gitignore create mode 100644 webots_ros2_driver/webots/README.md create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/accelerometer.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/altimeter.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/brake.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/camera.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/camera_recognition_object.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/compass.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/connector.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/console.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/contact_point.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/device.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/display.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/distance_sensor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/emitter.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/gps.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/gyro.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/inertial_unit.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/joystick.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/keyboard.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/led.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/lidar.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/lidar_point.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/light_sensor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/microphone.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/motor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/mouse.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/mouse_state.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/nodes.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/pen.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/default.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_window.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_wwi.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/position_sensor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/radar.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/radar_target.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/radio.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/range_finder.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/receiver.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/remote_control.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/robot.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/skin.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/speaker.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/supervisor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/touch_sensor.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/types.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/utils/ansi_codes.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/utils/motion.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/utils/string.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/utils/system.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/vehicle/car.h create mode 100644 webots_ros2_driver/webots/include/controller/c/webots/vehicle/driver.h create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Accelerometer.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Altimeter.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Brake.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Camera.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Compass.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Connector.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Device.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Display.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/DistanceSensor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Emitter.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/GPS.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Gyro.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/ImageRef.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/InertialUnit.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Joystick.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Keyboard.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/LED.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Lidar.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/LightSensor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Motor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Mouse.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Pen.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/PositionSensor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Radar.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/RangeFinder.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Receiver.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Robot.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Skin.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Speaker.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/Supervisor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/TouchSensor.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/utils/AnsiCodes.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/utils/Motion.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Car.hpp create mode 100644 webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Driver.hpp create mode 100644 webots_ros2_driver/webots/include/plugins/physics.h create mode 100644 webots_ros2_driver/webots/include/plugins/radio.h create mode 100644 webots_ros2_driver/webots/lib/controller/.gitignore create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/.gitignore create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_ANGULAR.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_ENCODING_ERROR.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_READY.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_RECORDING.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SAVING.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SIMULATION_ERROR.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_WRITE_ERROR.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/allincludes.h create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/finish.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/launcher.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_values.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_camera_get_image.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_objects.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_segmentation_image.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_values.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_display_draw_polygon.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_display_fill_polygon.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_display_image_new.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_display_set_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_distance_sensor_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_emitter_send.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_speed_vector.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_values.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_values.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_quaternion.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_roll_pitch_yaw.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_point_cloud.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_range_image.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_point_cloud.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_range_image.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_light_sensor_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_mouse_get_state.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_pen_set_ink_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_radar_get_targets.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_get_range_image.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_image_get_depth.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_data.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_emitter_direction.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_robot_cleanup.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_robot_init.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_orientation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_position.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_orientation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_position.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_rotation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec2f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec3f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_rotation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec2f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec3f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_rotation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec2f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec3f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_rotation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec2f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec3f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_color.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_rotation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec2f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec3f.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_get_movie_status.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_load_world.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_movie_get_status.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_center_of_mass.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_point.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_points.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_orientation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_pose.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_position.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_velocity.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_save_world.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_set_label.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_physics_reset.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_revert.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_start_movie.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_stop_movie.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_orientation.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_position.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_world_save.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_lookup_table.m create mode 100644 webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_values.m create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/__init__.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/accelerometer.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/altimeter.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/ansi_codes.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/brake.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/camera.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/compass.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/connector.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/constants.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/device.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/display.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/distance_sensor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/emitter.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/field.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/gps.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/gyro.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/inertial_unit.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/joystick.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/keyboard.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/led.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/lidar.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/lidar_point.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/light_sensor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/motion.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/motor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/mouse.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/node.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/pen.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/position_sensor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/radar.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/radar_target.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/range_finder.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/receiver.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/robot.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/sensor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/skin.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/speaker.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/supervisor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/touch_sensor.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/controller/wb.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/vehicle/__init__.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/vehicle/car.py create mode 100644 webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/car.def create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car.c create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car_private.h create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/driver.def create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/src/driver.c create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/src/Car.cpp create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/src/Driver.cpp create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/java/.gitignore create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/java/Makefile create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/java/vehicle.i create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle310.cpp create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle37.cpp create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle38.cpp create mode 100644 webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle39.cpp create mode 100644 webots_ros2_driver/webots/resources/Makefile.include create mode 100644 webots_ros2_driver/webots/resources/Makefile.os.include create mode 100644 webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/Makefile create mode 100644 webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/generic.c create mode 100644 webots_ros2_driver/webots/src/controller/Makefile create mode 100644 webots_ros2_driver/webots/src/controller/c/Controller.def create mode 100644 webots_ros2_driver/webots/src/controller/c/Makefile create mode 100644 webots_ros2_driver/webots/src/controller/c/README.TXT create mode 100644 webots_ros2_driver/webots/src/controller/c/abstract_camera.c create mode 100644 webots_ros2_driver/webots/src/controller/c/abstract_camera.h create mode 100644 webots_ros2_driver/webots/src/controller/c/accelerometer.c create mode 100644 webots_ros2_driver/webots/src/controller/c/altimeter.c create mode 100644 webots_ros2_driver/webots/src/controller/c/ansi_codes.c create mode 100644 webots_ros2_driver/webots/src/controller/c/base64.c create mode 100644 webots_ros2_driver/webots/src/controller/c/base64.h create mode 100644 webots_ros2_driver/webots/src/controller/c/brake.c create mode 100644 webots_ros2_driver/webots/src/controller/c/camera.c create mode 100644 webots_ros2_driver/webots/src/controller/c/camera_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/compass.c create mode 100644 webots_ros2_driver/webots/src/controller/c/connector.c create mode 100644 webots_ros2_driver/webots/src/controller/c/console.c create mode 100644 webots_ros2_driver/webots/src/controller/c/default_robot_window.c create mode 100644 webots_ros2_driver/webots/src/controller/c/default_robot_window_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/device.c create mode 100644 webots_ros2_driver/webots/src/controller/c/device_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/display.c create mode 100644 webots_ros2_driver/webots/src/controller/c/display_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/distance_sensor.c create mode 100644 webots_ros2_driver/webots/src/controller/c/dynamic_library.c create mode 100644 webots_ros2_driver/webots/src/controller/c/dynamic_library.h create mode 100644 webots_ros2_driver/webots/src/controller/c/emitter.c create mode 100644 webots_ros2_driver/webots/src/controller/c/file.c create mode 100644 webots_ros2_driver/webots/src/controller/c/file.h create mode 100644 webots_ros2_driver/webots/src/controller/c/g_image.c create mode 100644 webots_ros2_driver/webots/src/controller/c/g_image.h create mode 100644 webots_ros2_driver/webots/src/controller/c/g_pipe.c create mode 100644 webots_ros2_driver/webots/src/controller/c/g_pipe.h create mode 100644 webots_ros2_driver/webots/src/controller/c/gps.c create mode 100644 webots_ros2_driver/webots/src/controller/c/gyro.c create mode 100644 webots_ros2_driver/webots/src/controller/c/html_robot_window.c create mode 100644 webots_ros2_driver/webots/src/controller/c/html_robot_window_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/image.c create mode 100644 webots_ros2_driver/webots/src/controller/c/image_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/inertial_unit.c create mode 100644 webots_ros2_driver/webots/src/controller/c/joystick.c create mode 100644 webots_ros2_driver/webots/src/controller/c/joystick_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/keyboard.c create mode 100644 webots_ros2_driver/webots/src/controller/c/keyboard_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/led.c create mode 100644 webots_ros2_driver/webots/src/controller/c/lidar.c create mode 100644 webots_ros2_driver/webots/src/controller/c/light_sensor.c create mode 100644 webots_ros2_driver/webots/src/controller/c/messages.h create mode 100644 webots_ros2_driver/webots/src/controller/c/microphone.c create mode 100644 webots_ros2_driver/webots/src/controller/c/motion.c create mode 100644 webots_ros2_driver/webots/src/controller/c/motion_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/motor.c create mode 100644 webots_ros2_driver/webots/src/controller/c/mouse.c create mode 100644 webots_ros2_driver/webots/src/controller/c/mouse_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/node.c create mode 100644 webots_ros2_driver/webots/src/controller/c/pen.c create mode 100644 webots_ros2_driver/webots/src/controller/c/percent.c create mode 100644 webots_ros2_driver/webots/src/controller/c/percent.h create mode 100644 webots_ros2_driver/webots/src/controller/c/position_sensor.c create mode 100644 webots_ros2_driver/webots/src/controller/c/radar.c create mode 100644 webots_ros2_driver/webots/src/controller/c/radio.c create mode 100644 webots_ros2_driver/webots/src/controller/c/range_finder.c create mode 100644 webots_ros2_driver/webots/src/controller/c/receiver.c create mode 100644 webots_ros2_driver/webots/src/controller/c/remote_control.c create mode 100644 webots_ros2_driver/webots/src/controller/c/remote_control_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/request.c create mode 100644 webots_ros2_driver/webots/src/controller/c/request.h create mode 100644 webots_ros2_driver/webots/src/controller/c/robot.c create mode 100644 webots_ros2_driver/webots/src/controller/c/robot_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/robot_window.c create mode 100644 webots_ros2_driver/webots/src/controller/c/robot_window_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/scheduler.c create mode 100644 webots_ros2_driver/webots/src/controller/c/scheduler.h create mode 100644 webots_ros2_driver/webots/src/controller/c/skin.c create mode 100644 webots_ros2_driver/webots/src/controller/c/speaker.c create mode 100644 webots_ros2_driver/webots/src/controller/c/string.c create mode 100644 webots_ros2_driver/webots/src/controller/c/supervisor.c create mode 100644 webots_ros2_driver/webots/src/controller/c/supervisor_private.h create mode 100644 webots_ros2_driver/webots/src/controller/c/system.c create mode 100644 webots_ros2_driver/webots/src/controller/c/tcp_client.c create mode 100644 webots_ros2_driver/webots/src/controller/c/tcp_client.h create mode 100644 webots_ros2_driver/webots/src/controller/c/touch_sensor.c create mode 100644 webots_ros2_driver/webots/src/controller/cpp/.gitignore create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Accelerometer.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Altimeter.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Brake.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Camera.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Compass.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Connector.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Device.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Display.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/DistanceSensor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Emitter.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Field.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/GPS.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Gyro.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/InertialUnit.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Joystick.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Keyboard.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/LED.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Lidar.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/LightSensor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Makefile create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Motion.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Motor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Mouse.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Node.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Pen.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/PositionSensor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Radar.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/RangeFinder.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Receiver.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Robot.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Skin.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Speaker.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/Supervisor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/cpp/TouchSensor.cpp create mode 100644 webots_ros2_driver/webots/src/controller/java/.gitignore create mode 100644 webots_ros2_driver/webots/src/controller/java/Makefile create mode 100644 webots_ros2_driver/webots/src/controller/java/controller.i create mode 100644 webots_ros2_driver/webots/src/controller/matlab/Makefile create mode 100755 webots_ros2_driver/webots/src/controller/matlab/mgenerate.py create mode 100644 webots_ros2_driver/webots/src/stb/.travis.yml create mode 100644 webots_ros2_driver/webots/src/stb/README.md create mode 100644 webots_ros2_driver/webots/src/stb/data/atari_8bit_font_revised.png create mode 100644 webots_ros2_driver/webots/src/stb/data/easy_font_raw.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/license.txt create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_caves_limit_connectivity.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_caves_tiny_corridors.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_corner_caves.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_horizontal_corridors_v1.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_horizontal_corridors_v2.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_horizontal_corridors_v3.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_limit_connectivity_fat.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_limited_connectivity.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_maze_2_wide.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_maze_plus_2_wide.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_open_areas.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_ref2_corner_caves.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_rooms_and_corridors.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_rooms_and_corridors_2_wide_diagonal_bias.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_rooms_limit_connectivity.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_round_rooms_diagonal_corridors.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_sean_dungeon.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_simple_caves_2_wide.png create mode 100644 webots_ros2_driver/webots/src/stb/data/herringbone/template_square_rooms_with_random_rects.png create mode 100644 webots_ros2_driver/webots/src/stb/data/map_01.png create mode 100644 webots_ros2_driver/webots/src/stb/data/map_02.png create mode 100644 webots_ros2_driver/webots/src/stb/data/map_03.png create mode 100644 webots_ros2_driver/webots/src/stb/deprecated/rrsprintf.h create mode 100644 webots_ros2_driver/webots/src/stb/deprecated/stb_image.c create mode 100644 webots_ros2_driver/webots/src/stb/deprecated/stretchy_buffer.txt create mode 100644 webots_ros2_driver/webots/src/stb/docs/other_libs.md create mode 100644 webots_ros2_driver/webots/src/stb/docs/stb_howto.txt create mode 100644 webots_ros2_driver/webots/src/stb/docs/stb_voxel_render_interview.md create mode 100644 webots_ros2_driver/webots/src/stb/docs/why_public_domain.md create mode 100644 webots_ros2_driver/webots/src/stb/stb.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_c_lexer.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_connected_components.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_divide.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_ds.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_dxt.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_easy_font.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_herringbone_wang_tile.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_image.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_image_resize.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_image_write.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_include.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_leakcheck.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_perlin.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_rect_pack.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_sprintf.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_textedit.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_tilemap_editor.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_truetype.h create mode 100644 webots_ros2_driver/webots/src/stb/stb_vorbis.c create mode 100644 webots_ros2_driver/webots/src/stb/stb_voxel_render.h create mode 100644 webots_ros2_driver/webots/src/stb/stretchy_buffer.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/Makefile create mode 100644 webots_ros2_driver/webots/src/stb/tests/c_lexer_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/c_lexer_test.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/README.md create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/cave_main.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/cave_mesher.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/cave_parse.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/cave_parse.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/cave_render.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/caveview.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/caveview.dsw create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/caveview.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/glext.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/glext_list.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/main.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/stb_gl.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/stb_glprog.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/caveview/win32/SDL_windows_main.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/grid_reachability.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/herringbone.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/herringbone_generator.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/herringbone_map.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/herringbone_map.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/image_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/image_test.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/image_write_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/oversample/README.md create mode 100644 webots_ros2_driver/webots/src/stb/tests/oversample/main.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/oversample/oversample.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/oversample/oversample.dsw create mode 100644 webots_ros2_driver/webots/src/stb/tests/oversample/stb_wingraph.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/pg_test/pg_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basi0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basi2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basi4a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basi6a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basn0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basn2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basn4a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/basn6a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/bgai4a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/bgan6a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/bggn4a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/bgyn6a16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi1n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi1n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi2n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi2n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi4n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi4n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi9n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/oi9n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/tbbn2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/tbgn2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/16bit/tbwn0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/PngSuite.LICENSE create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xc1n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xc9n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xcrn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xcsn0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xd0n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xd3n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xd9n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xdtn0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xhdn0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xlfn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xs1n0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xs2n0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xs4n0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/corrupt/xs7n0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi0g02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basi6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn0g02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/basn6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/bgai4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/bgan6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/bgbn4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/bgwn6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s01i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s01n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s02i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s02n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s03i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s03n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s04i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s04n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s05i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s05n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s06i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s06n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s07i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s07n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s08i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s08n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s09i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s09n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s32i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s32n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s33i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s33n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s34i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s34n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s35i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s35n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s36i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s36n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s37i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s37n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s38i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s38n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s39i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s39n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s40i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/s40n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbbn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbbn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbgn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbrn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbwn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tbyn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tm3n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tp0n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tp0n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tp0n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/tp1n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/z00n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/z03n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/z06n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary/z09n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi0g02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basi6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn0g01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn0g02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/basn6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/bgai4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/bgan6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/bgbn4a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/bgwn6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s01i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s01n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s02i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s02n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s03i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s03n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s04i3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s04n3p01.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s05i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s05n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s06i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s06n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s07i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s07n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s08i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s08n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s09i3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s09n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s32i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s32n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s33i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s33n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s34i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s34n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s35i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s35n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s36i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s36n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s37i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s37n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s38i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s38n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s39i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s39n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s40i3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/s40n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbbn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbbn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbgn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbrn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbwn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tbyn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tm3n3p02.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tp0n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tp0n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tp0n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/tp1n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/z00n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/z03n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/z06n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/primary_check/z09n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ccwn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ccwn3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cdfn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cdhn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cdsn2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cdun2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ch1n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ch2n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cm0n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cm7n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cm9n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs3n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs3n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs5n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs5n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs8n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cs8n3p08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ct0n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ct1n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cten0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ctfn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ctgn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/cthn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ctjn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ctzn0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f00n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f00n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f01n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f01n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f02n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f02n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f03n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f03n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f04n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f04n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/f99n0g04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g03n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g03n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g03n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g04n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g04n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g04n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g05n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g05n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g05n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g07n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g07n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g07n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g10n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g10n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g10n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g25n0g16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g25n2c08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/g25n3p04.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/pp0n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/pp0n6a08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ps1n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ps1n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ps2n0g08.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/pngsuite/unused/ps2n2c16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/prerelease/stb_lib.h create mode 100644 webots_ros2_driver/webots/src/stb/tests/resample_test.cpp create mode 100644 webots_ros2_driver/webots/src/stb/tests/resample_test_c.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/resize.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/sdf/sdf_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/sdf/sdf_test_arial_16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/sdf/sdf_test_times_16.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/sdf/sdf_test_times_50.png create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb.dsw create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb_cpp.cpp create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb_cpp.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/stb_static.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/stblib.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/stblib_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/stblib_test_companion.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/stretch_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/stretch_test.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tests/stretchy_buffer_test.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test.sbm create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_c_compilation.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_c_lexer.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_cpp_compilation.cpp create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_ds.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_ds_cpp.cpp create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_dxt.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_easyfont.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_image.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_image_write.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_perlin.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_siphash.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_sprintf.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_truetype.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_vorbis.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/test_voxel.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/textedit_sample.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/tilemap_editor_integration_example.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/vorbseek/vorbseek.c create mode 100644 webots_ros2_driver/webots/src/stb/tests/vorbseek/vorbseek.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tools/README.footer.md create mode 100644 webots_ros2_driver/webots/src/stb/tools/README.header.md create mode 100644 webots_ros2_driver/webots/src/stb/tools/README.list create mode 100644 webots_ros2_driver/webots/src/stb/tools/build_matrix.c create mode 100644 webots_ros2_driver/webots/src/stb/tools/easy_font_maker.c create mode 100644 webots_ros2_driver/webots/src/stb/tools/make_readme.c create mode 100644 webots_ros2_driver/webots/src/stb/tools/make_readme.dsp create mode 100644 webots_ros2_driver/webots/src/stb/tools/mr.bat create mode 100644 webots_ros2_driver/webots/src/stb/tools/unicode.c create mode 100644 webots_ros2_driver/webots/src/stb/tools/unicode/unicode.dsp diff --git a/.gitmodules b/.gitmodules index c50472ca7..e80f51e24 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,6 +1,3 @@ [submodule "webots_ros2_importer/webots_ros2_importer/urdf2webots"] path = webots_ros2_importer/webots_ros2_importer/urdf2webots url = https://github.com/cyberbotics/urdf2webots.git -[submodule "webots_ros2_driver/webots"] - path = webots_ros2_driver/webots - url = https://github.com/cyberbotics/webots-libcontroller.git diff --git a/webots_ros2/CHANGELOG.rst b/webots_ros2/CHANGELOG.rst index 7429cb6b8..7d9fd36cf 100644 --- a/webots_ros2/CHANGELOG.rst +++ b/webots_ros2/CHANGELOG.rst @@ -4,6 +4,9 @@ Changelog for package webots_ros2 2023.0.0 (2022-XX-XX) ------------------ +* Add support for the new Python API of Webots R2023a +* Convert C++ controller API functions to C +* Replace libController submodule by commited source files * Removed 'webots_ros2_core' package (deprecated). 2022.1.4 (2022-11-18) diff --git a/webots_ros2_control/CHANGELOG.rst b/webots_ros2_control/CHANGELOG.rst index 606300736..813c02195 100644 --- a/webots_ros2_control/CHANGELOG.rst +++ b/webots_ros2_control/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package webots_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.0.0 (2022-XX-XX) +------------------ +* Convert C++ controller API functions to C + 1.2.3 (2022-05-30) ------------------ * Fixed support for Humble and Rolling. diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index 68872601c..bc85cc84e 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -33,8 +33,8 @@ #include "webots_ros2_driver/WebotsNode.hpp" #include "webots_ros2_control/Ros2ControlSystemInterface.hpp" -#include "webots/Motor.hpp" -#include "webots/PositionSensor.hpp" +#include "webots/motor.h" +#include "webots/position_sensor.h" namespace webots_ros2_control { @@ -49,8 +49,8 @@ namespace webots_ros2_control bool controlVelocity; bool controlEffort; std::string name; - webots::Motor* motor; - webots::PositionSensor* sensor; + WbDeviceTag motor; + WbDeviceTag sensor; }; class Ros2ControlSystem : public Ros2ControlSystemInterface diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp index 463136b4d..f4c82091a 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp @@ -29,7 +29,7 @@ #include "hardware_interface/hardware_info.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "webots_ros2_driver/PluginInterface.hpp" -#include +#include #include "webots_ros2_driver/WebotsNode.hpp" namespace webots_ros2_control diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index 9b98b21b2..e705244e7 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -26,6 +26,8 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_list_macros.hpp" +#include + #include const double CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS = 1.0; @@ -35,7 +37,7 @@ namespace webots_ros2_control void Ros2Control::step() { - const int nowMs = mNode->robot()->getTime() * 1000.0; + const int nowMs = wb_robot_get_time() * 1000.0; const int periodMs = nowMs - mLastControlUpdateMs; const rclcpp::Duration dt = rclcpp::Duration::from_seconds(mControlPeriodMs / 1000.0); if (periodMs >= mControlPeriodMs) @@ -116,12 +118,12 @@ namespace webots_ros2_control const int updateRate = mControllerManager->get_parameter("update_rate").as_int(); mControlPeriodMs = (1.0 / updateRate) * 1000.0; - int controlPeriodProductMs = mNode->robot()->getBasicTimeStep(); + int controlPeriodProductMs = wb_robot_get_basic_time_step(); while (controlPeriodProductMs < mControlPeriodMs) - controlPeriodProductMs += mNode->robot()->getBasicTimeStep(); + controlPeriodProductMs += wb_robot_get_basic_time_step(); if (abs(controlPeriodProductMs - mControlPeriodMs) > CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS) - RCLCPP_WARN_STREAM(node->get_logger(), "Desired controller update period (" << mControlPeriodMs << "ms / " << updateRate << "Hz) is different from the Webots timestep (" << mNode->robot()->getBasicTimeStep() << "ms). Please adjust the `update_rate` parameter in the `controller_manager` or the `basicTimeStep` parameter in the Webots `WorldInfo` node."); + RCLCPP_WARN_STREAM(node->get_logger(), "Desired controller update period (" << mControlPeriodMs << "ms / " << updateRate << "Hz) is different from the Webots timestep (" << wb_robot_get_basic_time_step() << "ms). Please adjust the `update_rate` parameter in the `controller_manager` or the `basicTimeStep` parameter in the Webots `WorldInfo` node."); // Spin mExecutor->add_node(mControllerManager); diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 5b0400534..6b564b912 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -24,6 +24,9 @@ #include "rclcpp/rclcpp.hpp" #include "pluginlib/class_list_macros.hpp" +#include +#include + namespace webots_ros2_control { void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) @@ -34,12 +37,12 @@ namespace webots_ros2_control Joint joint; joint.name = component.name; - webots::Motor *motor = mNode->robot()->getMotor(joint.name); - webots::PositionSensor *sensor = mNode->robot()->getPositionSensor(joint.name); - joint.motor = (motor) ? motor : sensor->getMotor(); - joint.sensor = (sensor) ? sensor : motor->getPositionSensor(); + WbDeviceTag device = wb_robot_get_device(joint.name.c_str()); + WbNodeType type = wb_device_get_node_type(device); + joint.motor = (type == WB_NODE_LINEAR_MOTOR || type == WB_NODE_ROTATIONAL_MOTOR) ? device : wb_position_sensor_get_motor(device); + joint.sensor = (type == WB_NODE_POSITION_SENSOR) ? device : wb_motor_get_position_sensor(device); if (joint.sensor) - joint.sensor->enable(node->robot()->getBasicTimeStep()); + wb_position_sensor_enable(joint.sensor, wb_robot_get_basic_time_step()); if (!joint.sensor && !joint.motor) throw std::runtime_error("Cannot find a Motor or PositionSensor with name " + joint.name); @@ -68,8 +71,8 @@ namespace webots_ros2_control } if (joint.motor && joint.controlVelocity && !joint.controlPosition) { - joint.motor->setPosition(INFINITY); - joint.motor->setVelocity(0.0); + wb_motor_set_position(joint.motor, INFINITY); + wb_motor_set_velocity(joint.motor, 0.0); } mJoints.push_back(joint); @@ -158,13 +161,13 @@ namespace webots_ros2_control { static double lastReadTime = 0; - const double deltaTime = mNode->robot()->getTime() - lastReadTime; - lastReadTime = mNode->robot()->getTime(); + const double deltaTime = wb_robot_get_time() - lastReadTime; + lastReadTime = wb_robot_get_time(); for (Joint &joint : mJoints) { if (joint.sensor) { - const double position = joint.sensor->getValue(); + const double position = wb_position_sensor_get_value(joint.sensor); const double velocity = std::isnan(joint.position) ? NAN : (position - joint.position) / deltaTime; if (!std::isnan(joint.velocity)) @@ -188,15 +191,15 @@ namespace webots_ros2_control if (joint.motor) { if (joint.controlPosition && !std::isnan(joint.positionCommand)) - joint.motor->setPosition(joint.positionCommand); + wb_motor_set_position(joint.motor, joint.positionCommand); if (joint.controlVelocity && !std::isnan(joint.velocityCommand)) { // In the position control mode the velocity cannot be negative. const double velocityCommand = joint.controlPosition ? abs(joint.velocityCommand) : joint.velocityCommand; - joint.motor->setVelocity(velocityCommand); + wb_motor_set_velocity(joint.motor, velocityCommand); } if (joint.controlEffort && !std::isnan(joint.effortCommand)) - joint.motor->setTorque(joint.effortCommand); + wb_motor_set_torque(joint.motor, joint.effortCommand); } } return hardware_interface::return_type::OK; diff --git a/webots_ros2_driver/CHANGELOG.rst b/webots_ros2_driver/CHANGELOG.rst index 3fdf5bc07..5de024515 100644 --- a/webots_ros2_driver/CHANGELOG.rst +++ b/webots_ros2_driver/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package webots_ros2_driver ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2023.0.0 (2022-XX-XX) +------------------ +* Add support for the new Python API of Webots R2023a +* Convert C++ controller API functions to C +* Replace libController submodule by commited source files + 2022.1.4 (2022-11-18) ------------------ * Fix the camera focal length in the CameraInfo topic. diff --git a/webots_ros2_driver/CMakeLists.txt b/webots_ros2_driver/CMakeLists.txt index d84995e52..c9bf997dd 100644 --- a/webots_ros2_driver/CMakeLists.txt +++ b/webots_ros2_driver/CMakeLists.txt @@ -45,72 +45,42 @@ else() find_package(PythonLibs 3.10 EXACT REQUIRED) endif() -if(UNIX AND NOT APPLE) - set(WEBOTS_LIB_BASE webots/lib/linux-gnu) -endif() +add_custom_target(compile-lib-controller ALL + COMMAND ${CMAKE_COMMAND} -E env "WEBOTS_HOME=${CMAKE_CURRENT_SOURCE_DIR}/webots" make release -f Makefile > /dev/null 2>&1 + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/webots/src/controller +) -if(APPLE) - set(WEBOTS_LIB_BASE webots/lib/darwin19) -endif() +add_custom_target(compile-lib-vehicle ALL + COMMAND ${CMAKE_COMMAND} -E env "WEBOTS_HOME=${CMAKE_CURRENT_SOURCE_DIR}/webots" make release -f Makefile > /dev/null 2>&1 + DEPENDS compile-lib-controller + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/webots/projects/default/libraries/vehicle +) -if(MSVC OR MSYS OR MINGW OR WIN32) - set(WEBOTS_LIB_BASE webots/lib/msys) -endif() +set(WEBOTS_LIB_BASE webots/lib/controller) include_directories( include - webots/include/c - webots/include/cpp + webots/include/controller/c + webots/include/controller/cpp ${PYTHON_INCLUDE_DIRS} ) link_directories(${WEBOTS_LIB_BASE}) -if(MSVC OR MSYS OR MINGW OR WIN32) - # Windows requires the libController C++ part to be compiled. - # See more here: https://cyberbotics.com/doc/guide/using-your-ide#visual-studio - set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - file(GLOB CppController_SRC CONFIGURE_DEPENDS "webots/source/cpp/*.cpp") - file(GLOB CppCarDriver_SRC CONFIGURE_DEPENDS "webots/source/cpp/vehicle/*.cpp") - add_library( - CppControllerRos - SHARED - ${CppController_SRC} - ${CppCarDriver_SRC} - ) - target_link_libraries(CppControllerRos - ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}driver${CMAKE_SHARED_LIBRARY_SUFFIX} - ) - install(TARGETS CppControllerRos - LIBRARY DESTINATION lib - ) - set(WEBOTS_LIB - ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}driver${CMAKE_SHARED_LIBRARY_SUFFIX} - CppControllerRos - ) - ament_export_libraries(CppControllerRos) -else() - set(WEBOTS_LIB - ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}driver${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}CppDriver${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX} - ${CMAKE_SHARED_LIBRARY_PREFIX}CppCar${CMAKE_SHARED_LIBRARY_SUFFIX} - ) -endif() +set(WEBOTS_LIB + ${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX} + ${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX} + ${CMAKE_SHARED_LIBRARY_PREFIX}driver${CMAKE_SHARED_LIBRARY_SUFFIX} + ${CMAKE_SHARED_LIBRARY_PREFIX}CppDriver${CMAKE_SHARED_LIBRARY_SUFFIX} + ${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX} + ${CMAKE_SHARED_LIBRARY_PREFIX}CppCar${CMAKE_SHARED_LIBRARY_SUFFIX} +) -if($ENV{ROS_DISTRO} MATCHES "foxy" OR $ENV{ROS_DISTRO} MATCHES "galactic") - ament_python_install_package(${PROJECT_NAME}_webots - PACKAGE_DIR ${WEBOTS_LIB_BASE}/python38) -else() - ament_python_install_package(${PROJECT_NAME}_webots - PACKAGE_DIR ${WEBOTS_LIB_BASE}/python310) -endif() +ament_python_install_package(controller + PACKAGE_DIR ${WEBOTS_LIB_BASE}/python/controller) + +ament_python_install_package(vehicle + PACKAGE_DIR ${WEBOTS_LIB_BASE}/python/vehicle) ament_python_install_package(${PROJECT_NAME} PACKAGE_DIR ${PROJECT_NAME}) @@ -145,6 +115,9 @@ ament_target_dependencies(driver tinyxml2_vendor TinyXML2 ) +add_dependencies(driver + compile-lib-vehicle +) target_link_libraries(driver ${WEBOTS_LIB} ${PYTHON_LIBRARIES} @@ -154,8 +127,7 @@ install( DESTINATION include ) install(TARGETS driver - RUNTIME - DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} ) # Dynamic IMU @@ -173,65 +145,51 @@ ament_target_dependencies(${PROJECT_NAME}_imu pluginlib tf2_ros ) +add_dependencies(${PROJECT_NAME}_imu + compile-lib-vehicle +) target_link_libraries(${PROJECT_NAME}_imu ${WEBOTS_LIB} ) +install(TARGETS ${PROJECT_NAME}_imu + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib +) +# libController install( - DIRECTORY webots/include/c + DIRECTORY webots/include/controller/c DESTINATION include/webots ) install( - DIRECTORY webots/include/cpp + DIRECTORY webots/include/controller/cpp DESTINATION include/webots ) -install(TARGETS ${PROJECT_NAME}_imu - RUNTIME DESTINATION bin - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib -) - -if(MSVC OR MSYS OR MINGW OR WIN32) - # Windows requires the library to be placed with executable - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}Controller${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}car${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION lib/${PROJECT_NAME} - ) - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}driver${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION lib/${PROJECT_NAME} - ) - - # Windows requires the C++ library to be placed with the Python module - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}CppController${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}_webots/" - ) - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}CppCar${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}_webots/" - ) - install( - FILES "${WEBOTS_LIB_BASE}/${CMAKE_SHARED_LIBRARY_PREFIX}CppDriver${CMAKE_SHARED_LIBRARY_SUFFIX}" - DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}_webots/" - ) -else() - install( - DIRECTORY ${WEBOTS_LIB_BASE}/ - DESTINATION lib - PATTERN "python*" EXCLUDE - PATTERN "*Controller*" - PATTERN "*CppController*" - PATTERN "*car*" - PATTERN "*CppCar*" - PATTERN "*driver*" - PATTERN "*CppDriver*" - ) -endif() +install( + DIRECTORY ${WEBOTS_LIB_BASE}/ + DESTINATION lib/controller + PATTERN "python*" EXCLUDE + PATTERN "matlab" EXCLUDE + PATTERN "*Controller*" + PATTERN "*CppController*" + PATTERN "*car*" + PATTERN "*CppCar*" + PATTERN "*driver*" + PATTERN "*CppDriver*" +) + +# Create symlinks of libController +macro(lib_symlink filename) + install(CODE "execute_process(COMMAND ln -sf ${CMAKE_INSTALL_PREFIX}/lib/controller/${filename} ${CMAKE_INSTALL_PREFIX}/lib/${filename})") +endmacro(lib_symlink) + +lib_symlink(libController.so) +lib_symlink(libcar.so) +lib_symlink(libCppCar.so) +lib_symlink(libCppController.so) +lib_symlink(libCppDriver.so) +lib_symlink(libdriver.so) # Prevent pluginlib from using boost target_compile_definitions(driver PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") diff --git a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp index d246fef88..8d28743d2 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp @@ -21,8 +21,7 @@ #include -#include -#include +#include #include #include @@ -38,10 +37,9 @@ namespace webots_ros2_driver class WebotsNode : public rclcpp::Node { public: - WebotsNode(std::string name, webots::Supervisor *robot); + WebotsNode(std::string name); void init(); int step(); - webots::Supervisor *robot() { return mRobot; } std::string urdf() const { return mRobotDescription; }; static void handleSignals(); @@ -60,7 +58,6 @@ namespace webots_ros2_driver rclcpp::TimerBase::SharedPtr mTimer; int mStep; - webots::Supervisor *mRobot; std::vector> mPlugins; pluginlib::ClassLoader mPluginLoader; tinyxml2::XMLElement *mWebotsXMLElement; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp index 4786b7cff..6e3b4fd3d 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp @@ -16,7 +16,7 @@ #define ROS2_SENSOR_PLUGIN_HPP #include -#include +#include #include #include diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp index ec55521ca..f9d5a244a 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp @@ -17,9 +17,9 @@ #include -#include -#include -#include +#include +#include +#include #include @@ -43,9 +43,9 @@ namespace webots_ros2_driver rclcpp::Publisher::SharedPtr mPublisher; sensor_msgs::msg::Imu mMessage; - webots::InertialUnit *mInertialUnit; - webots::Gyro *mGyro; - webots::Accelerometer *mAccelerometer; + WbDeviceTag mInertialUnit; + WbDeviceTag mGyro; + WbDeviceTag mAccelerometer; bool mIsEnabled; }; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp index 923054c50..313e5b677 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp @@ -17,7 +17,7 @@ #include -#include +#include #include #include @@ -51,7 +51,7 @@ namespace webots_ros2_driver void publishImage(); void publishRecognition(); - webots::Camera* mCamera; + WbDeviceTag mCamera; rclcpp::Publisher::SharedPtr mImagePublisher; sensor_msgs::msg::Image mImageMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp index 8ae239806..758d08b67 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -34,7 +34,7 @@ namespace webots_ros2_driver private: void publishRange(); - webots::DistanceSensor* mDistanceSensor; + WbDeviceTag mDistanceSensor; rclcpp::Publisher::SharedPtr mPublisher; sensor_msgs::msg::Range mMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp index 5c743c459..07bcae10c 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp @@ -15,7 +15,7 @@ #ifndef ROS2_GPS_HPP #define ROS2_GPS_HPP -#include +#include #include #include #include @@ -38,7 +38,7 @@ namespace webots_ros2_driver void publishSpeed(); void publishSpeedVector(); - webots::GPS *mGPS; + WbDeviceTag mGPS; rclcpp::Publisher::SharedPtr mGPSPublisher; sensor_msgs::msg::NavSatFix mGPSMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp index 5d459e2c6..a29bf5908 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include #include @@ -34,7 +34,7 @@ namespace webots_ros2_driver private: void onMessageReceived(const std_msgs::msg::Int32::SharedPtr message); - webots::LED *mLED; + WbDeviceTag mLED; webots_ros2_driver::WebotsNode *mNode; rclcpp::Subscription::SharedPtr mSubscriber; }; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp index e7c477856..c7ae55141 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp @@ -16,7 +16,7 @@ #define ROS2_LIDAR_HPP #include -#include +#include #include #include #include @@ -36,7 +36,7 @@ namespace webots_ros2_driver void publishPointCloud(); void publishLaserScan(); - webots::Lidar *mLidar; + WbDeviceTag mLidar; rclcpp::Publisher::SharedPtr mLaserPublisher; sensor_msgs::msg::LaserScan mLaserMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp index e5203902e..33633b456 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp @@ -17,7 +17,7 @@ #include #include -#include +#include #include #include @@ -35,7 +35,7 @@ namespace webots_ros2_driver void publishValue(); double findVariance(double rawValue); - webots::LightSensor* mLightSensor; + WbDeviceTag mLightSensor; rclcpp::Publisher::SharedPtr mPublisher; sensor_msgs::msg::Illuminance mMessage; diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp index 25fcb17c7..a898edf50 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include @@ -37,7 +37,7 @@ namespace webots_ros2_driver void publishImage(); void publishPointCloud(); - webots::RangeFinder* mRangeFinder; + WbDeviceTag mRangeFinder; rclcpp::Publisher::SharedPtr mImagePublisher; sensor_msgs::msg::Image mImageMessage; diff --git a/webots_ros2_driver/src/Driver.cpp b/webots_ros2_driver/src/Driver.cpp index 0cc9d771d..38bb0c639 100644 --- a/webots_ros2_driver/src/Driver.cpp +++ b/webots_ros2_driver/src/Driver.cpp @@ -14,18 +14,16 @@ #include #include +#include +#include #include -#include - -int main(int argc, char **argv) -{ - webots::Supervisor *robot; +int main(int argc, char **argv) { // Check if the robot can be a driver, if not create a simple Supervisor - if (webots::Driver::isInitialisationPossible()) - robot = new webots::Driver(); + if (wbu_driver_initialization_is_possible()) + wbu_driver_init(); else - robot = new webots::Supervisor(); + wb_robot_init(); // Let WebotsNode handle the system signals webots_ros2_driver::WebotsNode::handleSignals(); @@ -38,18 +36,19 @@ int main(int argc, char **argv) #endif rclcpp::init(argc, argv, options); - std::string robotName = robot->getName(); + std::string robotName(wb_robot_get_name()); for (char notAllowedChar : " -.)(") std::replace(robotName.begin(), robotName.end(), notAllowedChar, '_'); - std::shared_ptr node = std::make_shared(robotName, robot); + std::shared_ptr node = + std::make_shared(robotName); node->init(); while (true) { if (node->step() == -1) break; rclcpp::spin_some(node); } - delete robot; + wb_robot_cleanup(); rclcpp::shutdown(); return 0; } diff --git a/webots_ros2_driver/src/PythonPlugin.cpp b/webots_ros2_driver/src/PythonPlugin.cpp index dfc2e0a9b..af83742e4 100644 --- a/webots_ros2_driver/src/PythonPlugin.cpp +++ b/webots_ros2_driver/src/PythonPlugin.cpp @@ -31,19 +31,26 @@ namespace webots_ros2_driver R"EOT( import os import sys -import webots_ros2_driver_webots + +# Set WEBOTS_HOME to the webots_ros2_driver installation folder +# to load the correct libController libraries from the Python API +from ament_index_python.packages import get_package_prefix +os.environ['WEBOTS_HOME'] = get_package_prefix('webots_ros2_driver') + +import controller +from controller import Supervisor # As Driver need the controller library, we extend the path here # to avoid to load another library named "controller" when loading vehicle library -sys.path.insert(1, os.path.dirname(webots_ros2_driver_webots.__file__)) +sys.path.insert(1, os.path.dirname(controller.__file__)) from vehicle import Driver class WebotsNode: def __init__(self): if Driver.isInitialisationPossible(): - self.robot = Driver.getDriverInstance() + self.robot = Driver() else: - self.robot = Driver.getSupervisorInstance() + self.robot = Supervisor() )EOT", "webots_extra", Py_file_input); diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index ea394a5de..db90e8215 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -18,7 +18,8 @@ #include #include -#include +#include +#include #include "webots_ros2_driver/PluginInterface.hpp" #include @@ -46,7 +47,7 @@ namespace webots_ros2_driver gShutdownSignalReceived = true; } - WebotsNode::WebotsNode(std::string name, webots::Supervisor *robot) : Node(name), mRobot(robot), mPluginLoader(gPluginInterfaceName, gPluginInterface) + WebotsNode::WebotsNode(std::string name) : Node(name), mPluginLoader(gPluginInterfaceName, gPluginInterface) { mRobotDescription = this->declare_parameter("robot_description", ""); mSetRobotStatePublisher = this->declare_parameter("set_robot_state_publisher", false); @@ -121,48 +122,50 @@ namespace webots_ros2_driver void WebotsNode::init() { - if (mSetRobotStatePublisher) - setAnotherNodeParameter("robot_state_publisher", "robot_description", mRobot->getUrdf()); - - mStep = mRobot->getBasicTimeStep(); + if (mSetRobotStatePublisher){ + std::string prefix = ""; + setAnotherNodeParameter("robot_state_publisher", "robot_description", wb_robot_get_urdf(prefix.c_str())); + } + + mStep = wb_robot_get_basic_time_step(); // Load static plugins // Static plugins are automatically configured based on the robot model. // The static plugins will try to guess parameter based on the robot model, // but one can overwrite the default behavior in the section. // Typical static plugins are ROS 2 interfaces for Webots devices. - for (int i = 0; i < mRobot->getNumberOfDevices(); i++) + for (int i = 0; i < wb_robot_get_number_of_devices(); i++) { - webots::Device *device = mRobot->getDeviceByIndex(i); + WbDeviceTag device = wb_robot_get_device_by_index(i); // Prepare parameters - std::unordered_map parameters = getDeviceRosProperties(device->getName()); + std::unordered_map parameters = getDeviceRosProperties(wb_device_get_name(device)); if (parameters["enabled"] == "false") continue; - parameters["name"] = device->getName(); + parameters["name"] = wb_device_get_name(device); std::shared_ptr plugin = nullptr; - switch (device->getNodeType()) + switch (wb_device_get_node_type(device)) { - case webots::Node::LIDAR: + case WB_NODE_LIDAR: plugin = std::make_shared(); break; - case webots::Node::CAMERA: + case WB_NODE_CAMERA: plugin = std::make_shared(); break; - case webots::Node::GPS: + case WB_NODE_GPS: plugin = std::make_shared(); break; - case webots::Node::RANGE_FINDER: + case WB_NODE_RANGE_FINDER: plugin = std::make_shared(); break; - case webots::Node::DISTANCE_SENSOR: + case WB_NODE_DISTANCE_SENSOR: plugin = std::make_shared(); break; - case webots::Node::LIGHT_SENSOR: + case WB_NODE_LIGHT_SENSOR: plugin = std::make_shared(); break; - case webots::Node::LED: + case WB_NODE_LED: plugin = std::make_shared(); break; } @@ -229,7 +232,7 @@ namespace webots_ros2_driver mWaitingForUrdfRobotToBeRemoved = true; } - const int result = mRobot->step(mStep); + const int result = wb_robot_step(mStep); if (result == -1) return result; for (std::shared_ptr plugin : mPlugins) diff --git a/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp b/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp index 325a4e81d..8d59b3061 100644 --- a/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp +++ b/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp @@ -15,6 +15,8 @@ #include #include +#include + namespace webots_ros2_driver { void Ros2SensorPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) @@ -28,15 +30,15 @@ namespace webots_ros2_driver mFrameName = parameters.count("frameName") ? parameters["frameName"] : getFixedNameString(parameters["name"]); // Calculate timestep - mPublishTimestepSyncedMs = getDeviceTimestepMsFromPublishTimestep(mPublishTimestep, mNode->robot()->getBasicTimeStep()); + mPublishTimestepSyncedMs = getDeviceTimestepMsFromPublishTimestep(mPublishTimestep, wb_robot_get_basic_time_step()); } bool Ros2SensorPlugin::preStep() { // Update only if needed - if (mNode->robot()->getTime() - mLastUpdate < mPublishTimestep) + if (wb_robot_get_time() - mLastUpdate < mPublishTimestep) return false; - mLastUpdate = mNode->robot()->getTime(); + mLastUpdate = wb_robot_get_time(); return true; } } diff --git a/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp b/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp index f43d41f66..d8b99391b 100644 --- a/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp +++ b/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp @@ -17,37 +17,41 @@ #include #include "pluginlib/class_list_macros.hpp" +#include +#include + + namespace webots_ros2_driver { void Ros2IMU::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; - mInertialUnit = NULL; - mGyro = NULL; - mAccelerometer = NULL; + mInertialUnit = 0; + mGyro = 0; + mAccelerometer = 0; if (!parameters.count("inertialUnitName") && !parameters.count("gyroName") && !parameters.count("accelerometerName")) throw std::runtime_error("The IMU plugins has to contain at least of: , , or "); if (parameters.count("inertialUnitName")) { - mInertialUnit = mNode->robot()->getInertialUnit(parameters["inertialUnitName"]); - if (mInertialUnit == NULL) + mInertialUnit = wb_robot_get_device(parameters["inertialUnitName"].c_str()); + if (mInertialUnit == 0 || wb_device_get_node_type(mInertialUnit) != WB_NODE_INERTIAL_UNIT) throw std::runtime_error("Cannot find InertialUnit with name " + parameters["inertialUnitName"]); } if (parameters.count("gyroName")) { - mGyro = mNode->robot()->getGyro(parameters["gyroName"]); - if (mGyro == NULL) + mGyro = wb_robot_get_device(parameters["gyroName"].c_str()); + if (mGyro == 0 || wb_device_get_node_type(mGyro) != WB_NODE_GYRO) throw std::runtime_error("Cannot find Gyro with name " + parameters["gyroName"]); } if (parameters.count("accelerometerName")) { - mAccelerometer = mNode->robot()->getAccelerometer(parameters["accelerometerName"]); - if (mAccelerometer == NULL) + mAccelerometer = wb_robot_get_device(parameters["accelerometerName"].c_str()); + if (mAccelerometer == 0 || wb_device_get_node_type(mAccelerometer) != WB_NODE_ACCELEROMETER) throw std::runtime_error("Cannot find Accelerometer with name " + parameters["accelerometerName"]); } @@ -63,21 +67,21 @@ namespace webots_ros2_driver void Ros2IMU::enable() { if (mInertialUnit) - mInertialUnit->enable(mPublishTimestepSyncedMs); + wb_inertial_unit_enable(mInertialUnit, mPublishTimestepSyncedMs); if (mAccelerometer) - mAccelerometer->enable(mPublishTimestepSyncedMs); + wb_accelerometer_enable(mAccelerometer, mPublishTimestepSyncedMs); if (mGyro) - mGyro->enable(mPublishTimestepSyncedMs); + wb_gyro_enable(mGyro, mPublishTimestepSyncedMs); } void Ros2IMU::disable() { if (mInertialUnit) - mInertialUnit->disable(); + wb_inertial_unit_disable(mInertialUnit); if (mAccelerometer) - mAccelerometer->disable(); + wb_accelerometer_disable(mAccelerometer); if (mGyro) - mGyro->disable(); + wb_gyro_disable(mGyro); } void Ros2IMU::step() @@ -108,21 +112,21 @@ namespace webots_ros2_driver mMessage.header.stamp = mNode->get_clock()->now(); if (mAccelerometer) { - const double *values = mAccelerometer->getValues(); + const double *values = wb_accelerometer_get_values(mAccelerometer); mMessage.linear_acceleration.x = values[0]; mMessage.linear_acceleration.y = values[1]; mMessage.linear_acceleration.z = values[2]; } if (mGyro) { - const double *values = mGyro->getValues(); + const double *values = wb_gyro_get_values(mGyro); mMessage.angular_velocity.x = values[0]; mMessage.angular_velocity.y = values[1]; mMessage.angular_velocity.z = values[2]; } if (mInertialUnit) { - const double *values = mInertialUnit->getQuaternion(); + const double *values = wb_inertial_unit_get_quaternion(mInertialUnit); mMessage.orientation.x = values[0]; mMessage.orientation.y = values[1]; mMessage.orientation.z = values[2]; diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index ca69dbf2d..a13404da8 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp @@ -14,6 +14,8 @@ #include +#include + namespace webots_ros2_driver { void Ros2Camera::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) @@ -21,44 +23,44 @@ namespace webots_ros2_driver Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mRecognitionIsEnabled = false; - mCamera = mNode->robot()->getCamera(parameters["name"]); + mCamera = wb_robot_get_device(parameters["name"].c_str()); - assert(mCamera != NULL); + assert(mCamera != 0); // Image publisher mImagePublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mImageMessage.header.frame_id = mFrameName; - mImageMessage.height = mCamera->getHeight(); - mImageMessage.width = mCamera->getWidth(); + mImageMessage.height = wb_camera_get_height(mCamera); + mImageMessage.width = wb_camera_get_width(mCamera); mImageMessage.is_bigendian = false; - mImageMessage.step = sizeof(unsigned char) * 4 * mCamera->getWidth(); - mImageMessage.data.resize(4 * mCamera->getWidth() * mCamera->getHeight()); + mImageMessage.step = sizeof(unsigned char) * 4 * wb_camera_get_width(mCamera); + mImageMessage.data.resize(4 * wb_camera_get_width(mCamera) * wb_camera_get_height(mCamera)); mImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; // CameraInfo publisher mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; - mCameraInfoMessage.height = mCamera->getHeight(); - mCameraInfoMessage.width = mCamera->getWidth(); + mCameraInfoMessage.height = wb_camera_get_height(mCamera); + mCameraInfoMessage.width = wb_camera_get_width(mCamera); mCameraInfoMessage.distortion_model = "plumb_bob"; // Convert FoV to focal length. - const double focalLength = mCamera->getWidth() / (2 * tan(mCamera->getFov() / 2)); + const double focalLength = wb_camera_get_width(mCamera) / (2 * tan(wb_camera_get_fov(mCamera) / 2)); mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; mCameraInfoMessage.k = { - focalLength, 0.0, (double)mCamera->getWidth() / 2, - 0.0, focalLength, (double)mCamera->getHeight() / 2, + focalLength, 0.0, (double)wb_camera_get_width(mCamera) / 2, + 0.0, focalLength, (double)wb_camera_get_height(mCamera) / 2, 0.0, 0.0, 1.0}; mCameraInfoMessage.p = { - focalLength, 0.0, (double)mCamera->getWidth() / 2, 0.0, - 0.0, focalLength, (double)mCamera->getHeight() / 2, 0.0, + focalLength, 0.0, (double)wb_camera_get_width(mCamera) / 2, 0.0, + 0.0, focalLength, (double)wb_camera_get_height(mCamera) / 2, 0.0, 0.0, 0.0, 1.0, 0.0}; // Recognition publisher - if (mCamera->hasRecognition()) + if (wb_camera_has_recognition(mCamera)) { mRecognitionPublisher = mNode->create_publisher( mTopicName + "/recognitions", @@ -87,18 +89,18 @@ namespace webots_ros2_driver if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) - mCamera->enable(mPublishTimestepSyncedMs); + wb_camera_enable(mCamera, mPublishTimestepSyncedMs); else - mCamera->disable(); + wb_camera_disable(mCamera); mIsEnabled = shouldBeEnabled; } if (recognitionSubscriptionsExist != mRecognitionIsEnabled) { if (recognitionSubscriptionsExist) - mCamera->recognitionEnable(mPublishTimestepSyncedMs); + wb_camera_recognition_enable(mCamera, mPublishTimestepSyncedMs); else - mCamera->recognitionDisable(); + wb_camera_recognition_disable(mCamera); mRecognitionIsEnabled = recognitionSubscriptionsExist; } @@ -113,7 +115,7 @@ namespace webots_ros2_driver void Ros2Camera::publishImage() { - auto image = mCamera->getImage(); + auto image = wb_camera_get_image(mCamera); if (image) { mImageMessage.header.stamp = mNode->get_clock()->now(); @@ -124,16 +126,16 @@ namespace webots_ros2_driver void Ros2Camera::publishRecognition() { - if (mCamera->getRecognitionNumberOfObjects() == 0) + if (wb_camera_recognition_get_number_of_objects(mCamera) == 0) return; - auto objects = mCamera->getRecognitionObjects(); + auto objects = wb_camera_recognition_get_objects(mCamera); mRecognitionMessage.header.stamp = mNode->get_clock()->now(); mWebotsRecognitionMessage.header.stamp = mNode->get_clock()->now(); mRecognitionMessage.detections.clear(); mWebotsRecognitionMessage.objects.clear(); - for (size_t i = 0; i < mCamera->getRecognitionNumberOfObjects(); i++) + for (size_t i = 0; i < wb_camera_recognition_get_number_of_objects(mCamera); i++) { // Getting Object Info geometry_msgs::msg::PoseStamped pose; diff --git a/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp b/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp index 856495e4e..6108f1de3 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp @@ -16,17 +16,19 @@ #include +#include + namespace webots_ros2_driver { void Ros2DistanceSensor::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; - mDistanceSensor = mNode->robot()->getDistanceSensor(parameters["name"]); + mDistanceSensor = wb_robot_get_device(parameters["name"].c_str()); - assert(mDistanceSensor != NULL); + assert(mDistanceSensor != 0); - mLookupTable.assign(mDistanceSensor->getLookupTable(), mDistanceSensor->getLookupTable() + mDistanceSensor->getLookupTableSize() * 3); + mLookupTable.assign(wb_distance_sensor_get_lookup_table(mDistanceSensor), wb_distance_sensor_get_lookup_table(mDistanceSensor) + wb_distance_sensor_get_lookup_table_size(mDistanceSensor) * 3); const int size = mLookupTable.size(); const double maxValue = std::max(mLookupTable[0], mLookupTable[size - 3]); @@ -38,13 +40,13 @@ namespace webots_ros2_driver mPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mMessage.header.frame_id = mFrameName; - mMessage.field_of_view = mDistanceSensor->getAperture(); + mMessage.field_of_view = wb_distance_sensor_get_aperture(mDistanceSensor); mMessage.min_range = minRange; mMessage.max_range = maxRange; mMessage.radiation_type = sensor_msgs::msg::Range::INFRARED; if (mAlwaysOn) { - mDistanceSensor->enable(mPublishTimestepSyncedMs); + wb_distance_sensor_enable(mDistanceSensor, mPublishTimestepSyncedMs); mIsEnabled = true; } } @@ -65,16 +67,16 @@ namespace webots_ros2_driver if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) - mDistanceSensor->enable(mPublishTimestepSyncedMs); + wb_distance_sensor_enable(mDistanceSensor, mPublishTimestepSyncedMs); else - mDistanceSensor->disable(); + wb_distance_sensor_disable(mDistanceSensor); mIsEnabled = shouldBeEnabled; } } void Ros2DistanceSensor::publishRange() { - const double value = mDistanceSensor->getValue(); + const double value = wb_distance_sensor_get_value(mDistanceSensor); if (std::isnan(value)) return; mMessage.header.stamp = mNode->get_clock()->now(); diff --git a/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp b/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp index 8a9377ba0..6be2a38bd 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp @@ -14,6 +14,8 @@ #include +#include + namespace webots_ros2_driver { @@ -21,11 +23,11 @@ namespace webots_ros2_driver { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; - mGPS = mNode->robot()->getGPS(parameters["name"]); + mGPS = wb_robot_get_device(parameters["name"].c_str()); - assert(mGPS != NULL); + assert(mGPS != 0); - if (mGPS->getCoordinateSystem() == webots::GPS::WGS84) + if (wb_gps_get_coordinate_system(mGPS) == WB_GPS_WGS84_COORDINATE) { mGPSPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mGPSMessage.header.frame_id = mFrameName; @@ -41,7 +43,7 @@ namespace webots_ros2_driver mSpeedVectorPublisher = mNode->create_publisher(mTopicName + "/speed_vector", rclcpp::SensorDataQoS().reliable()); if (mAlwaysOn) { - mGPS->enable(mPublishTimestepSyncedMs); + wb_gps_enable(mGPS, mPublishTimestepSyncedMs); mIsEnabled = true; } } @@ -76,9 +78,9 @@ namespace webots_ros2_driver if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) - mGPS->enable(mPublishTimestepSyncedMs); + wb_gps_enable(mGPS, mPublishTimestepSyncedMs); else - mGPS->disable(); + wb_gps_disable(mGPS); mIsEnabled = shouldBeEnabled; } } @@ -86,7 +88,7 @@ namespace webots_ros2_driver void Ros2GPS::publishPoint() { mPointMessage.header.stamp = mNode->get_clock()->now(); - const double *values = mGPS->getValues(); + const double *values = wb_gps_get_values(mGPS); mPointMessage.point.x = values[0]; mPointMessage.point.y = values[1]; mPointMessage.point.z = values[2]; @@ -96,7 +98,7 @@ namespace webots_ros2_driver void Ros2GPS::publishGPS() { mGPSMessage.header.stamp = mNode->get_clock()->now(); - const double *values = mGPS->getValues(); + const double *values = wb_gps_get_values(mGPS); mGPSMessage.latitude = values[0]; mGPSMessage.longitude = values[1]; mGPSMessage.altitude = values[2]; @@ -105,13 +107,13 @@ namespace webots_ros2_driver void Ros2GPS::publishSpeed() { - mSpeedMessage.data = mGPS->getSpeed(); + mSpeedMessage.data = wb_gps_get_speed(mGPS); mSpeedPublisher->publish(mSpeedMessage); } void Ros2GPS::publishSpeedVector() { - const double *values = mGPS->getSpeedVector(); + const double *values = wb_gps_get_speed_vector(mGPS); mSpeedVectorMessage.x = values[0]; mSpeedVectorMessage.y = values[1]; mSpeedVectorMessage.z = values[2]; diff --git a/webots_ros2_driver/src/plugins/static/Ros2LED.cpp b/webots_ros2_driver/src/plugins/static/Ros2LED.cpp index 15d43765a..5972b3604 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2LED.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2LED.cpp @@ -16,14 +16,16 @@ #include +#include + namespace webots_ros2_driver { void Ros2LED::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { mNode = node; - mLED = mNode->robot()->getLED(parameters["name"]); + mLED = wb_robot_get_device(parameters["name"].c_str()); - assert(mLED != NULL); + assert(mLED != 0); const std::string topicName = parameters.count("topicName") ? parameters["topicName"] : "~/" + getFixedNameString(parameters["name"]); mSubscriber = mNode->create_subscription(topicName, rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2LED::onMessageReceived, this, std::placeholders::_1)); @@ -35,6 +37,6 @@ namespace webots_ros2_driver void Ros2LED::onMessageReceived(const std_msgs::msg::Int32::SharedPtr message) { - mLED->set(message->data); + wb_led_set(mLED, message->data); } } diff --git a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp index 363802c87..78236bc24 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp @@ -17,6 +17,8 @@ #include #include +#include + namespace webots_ros2_driver { void Ros2Lidar::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) @@ -24,23 +26,23 @@ namespace webots_ros2_driver Ros2SensorPlugin::init(node, parameters); mIsSensorEnabled = false; mIsPointCloudEnabled = false; - mLidar = mNode->robot()->getLidar(parameters["name"]); + mLidar = wb_robot_get_device(parameters["name"].c_str()); - assert(mLidar != NULL); + assert(mLidar != 0); // Laser publisher - if (mLidar->getNumberOfLayers() == 1) + if (wb_lidar_get_number_of_layers(mLidar) == 1) { mLaserPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); - const int resolution = mLidar->getHorizontalResolution(); + const int resolution = wb_lidar_get_horizontal_resolution(mLidar); mLaserMessage.header.frame_id = mFrameName; - mLaserMessage.angle_increment = -mLidar->getFov() / (resolution - 1); - mLaserMessage.angle_min = mLidar->getFov() / 2.0; - mLaserMessage.angle_max = -mLidar->getFov() / 2.0; - mLaserMessage.time_increment = (double)mLidar->getSamplingPeriod() / (1000.0 * resolution); - mLaserMessage.scan_time = (double)mLidar->getSamplingPeriod() / 1000.0; - mLaserMessage.range_min = mLidar->getMinRange(); - mLaserMessage.range_max = mLidar->getMaxRange(); + mLaserMessage.angle_increment = -wb_lidar_get_fov(mLidar) / (resolution - 1); + mLaserMessage.angle_min = wb_lidar_get_fov(mLidar) / 2.0; + mLaserMessage.angle_max = -wb_lidar_get_fov(mLidar) / 2.0; + mLaserMessage.time_increment = (double)wb_lidar_get_sampling_period(mLidar) / (1000.0 * resolution); + mLaserMessage.scan_time = (double)wb_lidar_get_sampling_period(mLidar) / 1000.0; + mLaserMessage.range_min = wb_lidar_get_min_range(mLidar); + mLaserMessage.range_max = wb_lidar_get_max_range(mLidar); mLaserMessage.ranges.resize(resolution); } @@ -66,8 +68,8 @@ namespace webots_ros2_driver mPointCloudMessage.is_bigendian = false; if (mAlwaysOn) { - mLidar->enable(mPublishTimestepSyncedMs); - mLidar->enablePointCloud(); + wb_lidar_enable(mLidar, mPublishTimestepSyncedMs); + wb_lidar_enable_point_cloud(mLidar); mIsSensorEnabled = true; mIsPointCloudEnabled = true; } @@ -95,9 +97,9 @@ namespace webots_ros2_driver if (shouldSensorBeEnabled != mIsSensorEnabled) { if (shouldSensorBeEnabled) - mLidar->enable(mPublishTimestepSyncedMs); + wb_lidar_enable(mLidar, mPublishTimestepSyncedMs); else - mLidar->disable(); + wb_lidar_disable(mLidar); mIsSensorEnabled = shouldSensorBeEnabled; } @@ -105,22 +107,22 @@ namespace webots_ros2_driver if (shouldPointCloudBeEnabled != mIsPointCloudEnabled) { if (shouldPointCloudBeEnabled) - mLidar->enablePointCloud(); + wb_lidar_enable_point_cloud(mLidar); else - mLidar->disablePointCloud(); + wb_lidar_disable_point_cloud(mLidar); mIsPointCloudEnabled = shouldPointCloudBeEnabled; } } void Ros2Lidar::publishPointCloud() { - auto data = mLidar->getPointCloud(); + auto data = wb_lidar_get_point_cloud(mLidar); if (data) { mPointCloudMessage.header.stamp = mNode->get_clock()->now(); - mPointCloudMessage.width = mLidar->getNumberOfPoints(); - mPointCloudMessage.row_step = 20 * mLidar->getNumberOfPoints(); + mPointCloudMessage.width = wb_lidar_get_number_of_points(mLidar); + mPointCloudMessage.row_step = 20 * wb_lidar_get_number_of_points(mLidar); if (mPointCloudMessage.data.size() != mPointCloudMessage.row_step * mPointCloudMessage.height) mPointCloudMessage.data.resize(mPointCloudMessage.row_step * mPointCloudMessage.height); @@ -131,7 +133,7 @@ namespace webots_ros2_driver void Ros2Lidar::publishLaserScan() { - auto rangeImage = mLidar->getLayerRangeImage(0); + auto rangeImage = wb_lidar_get_layer_range_image(mLidar, 0); if (rangeImage) { memcpy(mLaserMessage.ranges.data(), rangeImage, mLaserMessage.ranges.size() * sizeof(float)); diff --git a/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp b/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp index 0745a2586..847baec9c 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp @@ -16,6 +16,8 @@ #include +#include + namespace webots_ros2_driver { const double gIrradianceToIlluminance = 120.0; @@ -24,17 +26,17 @@ namespace webots_ros2_driver { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; - mLightSensor = mNode->robot()->getLightSensor(parameters["name"]); + mLightSensor = wb_robot_get_device(parameters["name"].c_str()); - assert(mLightSensor != NULL); + assert(mLightSensor != 0); mPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mMessage.header.frame_id = mFrameName; - mLookupTable.assign(mLightSensor->getLookupTable(), mLightSensor->getLookupTable() + mLightSensor->getLookupTableSize() * 3); + mLookupTable.assign(wb_light_sensor_get_lookup_table(mLightSensor), wb_light_sensor_get_lookup_table(mLightSensor) + wb_light_sensor_get_lookup_table_size(mLightSensor) * 3); if (mAlwaysOn) { - mLightSensor->enable(mPublishTimestepSyncedMs); + wb_light_sensor_enable(mLightSensor, mPublishTimestepSyncedMs); mIsEnabled = true; } } @@ -55,16 +57,16 @@ namespace webots_ros2_driver if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) - mLightSensor->enable(mPublishTimestepSyncedMs); + wb_light_sensor_enable(mLightSensor, mPublishTimestepSyncedMs); else - mLightSensor->disable(); + wb_light_sensor_disable(mLightSensor); mIsEnabled = shouldBeEnabled; } } void Ros2LightSensor::publishValue() { - const double value = mLightSensor->getValue(); + const double value = wb_light_sensor_get_value(mLightSensor); mMessage.header.stamp = mNode->get_clock()->now(); mMessage.illuminance = interpolateLookupTable(value, mLookupTable); mMessage.variance = findVariance(value); diff --git a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp index dc5755f0d..681553a00 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp @@ -17,18 +17,20 @@ #include #include +#include + namespace webots_ros2_driver { void Ros2RangeFinder::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; - mRangeFinder = mNode->robot()->getRangeFinder(parameters["name"]); + mRangeFinder = wb_robot_get_device(parameters["name"].c_str()); - assert(mRangeFinder != NULL); + assert(mRangeFinder != 0); - const int width = mRangeFinder->getWidth(); - const int height = mRangeFinder->getHeight(); + const int width = wb_range_finder_get_width(mRangeFinder); + const int height = wb_range_finder_get_height(mRangeFinder); // Image publisher mImagePublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); @@ -47,8 +49,8 @@ namespace webots_ros2_driver mCameraInfoMessage.height = height; mCameraInfoMessage.width = width; mCameraInfoMessage.distortion_model = "plumb_bob"; - const double focalLengthX = 0.5 * width * (1 / tan(0.5 * mRangeFinder->getFov())); - const double focalLengthY = 0.5 * height * (1 / tan(0.5 * mRangeFinder->getFov())); + const double focalLengthX = 0.5 * width * (1 / tan(0.5 * wb_range_finder_get_fov(mRangeFinder))); + const double focalLengthY = 0.5 * height * (1 / tan(0.5 * wb_range_finder_get_fov(mRangeFinder))); mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; mCameraInfoMessage.k = { @@ -84,7 +86,7 @@ namespace webots_ros2_driver mPointCloudMessage.data.resize(width * 12 * height); if (mAlwaysOn) { - mRangeFinder->enable(mPublishTimestepSyncedMs); + wb_range_finder_enable(mRangeFinder, mPublishTimestepSyncedMs); mIsEnabled = true; } } @@ -110,16 +112,16 @@ namespace webots_ros2_driver if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) - mRangeFinder->enable(mPublishTimestepSyncedMs); + wb_range_finder_enable(mRangeFinder, mPublishTimestepSyncedMs); else - mRangeFinder->disable(); + wb_range_finder_disable(mRangeFinder); mIsEnabled = shouldBeEnabled; } } void Ros2RangeFinder::publishImage() { - auto image = mRangeFinder->getRangeImage(); + auto image = wb_range_finder_get_range_image(mRangeFinder); if (image) { mImageMessage.header.stamp = mNode->get_clock()->now(); @@ -131,7 +133,7 @@ namespace webots_ros2_driver // To be redesigned when mRangeFinder->getPointCloud() will be implemented on Webots side. void Ros2RangeFinder::publishPointCloud() { - auto image = mRangeFinder->getRangeImage(); + auto image = wb_range_finder_get_range_image(mRangeFinder); if (image) { mPointCloudMessage.header.stamp = mNode->get_clock()->now(); diff --git a/webots_ros2_driver/webots b/webots_ros2_driver/webots deleted file mode 160000 index 847bb7af3..000000000 --- a/webots_ros2_driver/webots +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 847bb7af32dcc0d4d16d776768d63a7c8bab3db8 diff --git a/webots_ros2_driver/webots/.gitignore b/webots_ros2_driver/webots/.gitignore new file mode 100644 index 000000000..7dee13b4c --- /dev/null +++ b/webots_ros2_driver/webots/.gitignore @@ -0,0 +1,73 @@ +# Global rules # +################ + +# Build directories +build +com +webots_catkin_ws + +# Compiled source +*.exe +*.o +*.d +*.class +*.so +*.dylib +*.dll +*.lib +*.a +*.cof +*.hex +*.pyc +*.gch + +# Packages +*.7z +*.bz2 +*.dmg +*.gz +*.iso +*.jar +*.rar +*.tar +*.xz +*.zip + +# OS generated files +.DS_Store +.DS_Store? +._* +.Spotlight-V100 +.Trashes +.directory +ehthumbs.db +Thumbs.db +*.swp + +# Text editor backup files +*~ + +# Log files +*.log + +# Blender cache +*.blend1 + +# Webots generated files +*.cache + +# IDE files +.vscode + +# Local rules # +############### + +/.clang-format +/msys64 +/webots +/webots.lnk +/webots_debug_output.txt +/util + +# world thumbnail files +.*.jpg diff --git a/webots_ros2_driver/webots/README.md b/webots_ros2_driver/webots/README.md new file mode 100644 index 000000000..d616b0357 --- /dev/null +++ b/webots_ros2_driver/webots/README.md @@ -0,0 +1,4 @@ +# webots-libcontroller + +This branch is a subset of the Webots repository, containing the controller library only. +It is primarily created to allow compilation of the [`webots_ros2`](https://github.com/cyberbotics/webots_ros2) interface without dependency on the complete Webots repository. \ No newline at end of file diff --git a/webots_ros2_driver/webots/include/controller/c/webots/accelerometer.h b/webots_ros2_driver/webots/include/controller/c/webots/accelerometer.h new file mode 100644 index 000000000..1e834405e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/accelerometer.h @@ -0,0 +1,45 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/altimeter.h b/webots_ros2_driver/webots/include/controller/c/webots/altimeter.h new file mode 100644 index 000000000..feda2058e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/altimeter.h @@ -0,0 +1,41 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/brake.h b/webots_ros2_driver/webots/include/controller/c/webots/brake.h new file mode 100644 index 000000000..a8dff9617 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/brake.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/camera.h b/webots_ros2_driver/webots/include/controller/c/webots/camera.h new file mode 100644 index 000000000..b316da761 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/camera.h @@ -0,0 +1,104 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/camera_recognition_object.h b/webots_ros2_driver/webots/include/controller/c/webots/camera_recognition_object.h new file mode 100644 index 000000000..13c0a9ed8 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/camera_recognition_object.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/***************************************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/compass.h b/webots_ros2_driver/webots/include/controller/c/webots/compass.h new file mode 100644 index 000000000..08a91b18e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/compass.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/connector.h b/webots_ros2_driver/webots/include/controller/c/webots/connector.h new file mode 100644 index 000000000..40eb07b4f --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/connector.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/console.h b/webots_ros2_driver/webots/include/controller/c/webots/console.h new file mode 100644 index 000000000..70bc55058 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/console.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/include/controller/c/webots/contact_point.h b/webots_ros2_driver/webots/include/controller/c/webots/contact_point.h new file mode 100644 index 000000000..a544015e5 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/contact_point.h @@ -0,0 +1,25 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef WB_CONTACT_POINTS_H +#define WB_CONTACT_POINTS_H + +typedef struct { + double point[3]; + int node_id; +} WbContactPoint; + +#endif diff --git a/webots_ros2_driver/webots/include/controller/c/webots/device.h b/webots_ros2_driver/webots/include/controller/c/webots/device.h new file mode 100644 index 000000000..a3411d323 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/device.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/display.h b/webots_ros2_driver/webots/include/controller/c/webots/display.h new file mode 100644 index 000000000..49e1908ec --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/display.h @@ -0,0 +1,72 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/distance_sensor.h b/webots_ros2_driver/webots/include/controller/c/webots/distance_sensor.h new file mode 100644 index 000000000..e962d347a --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/distance_sensor.h @@ -0,0 +1,56 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/emitter.h b/webots_ros2_driver/webots/include/controller/c/webots/emitter.h new file mode 100644 index 000000000..17e1fd786 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/emitter.h @@ -0,0 +1,46 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/gps.h b/webots_ros2_driver/webots/include/controller/c/webots/gps.h new file mode 100644 index 000000000..86a9d7933 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/gps.h @@ -0,0 +1,49 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/gyro.h b/webots_ros2_driver/webots/include/controller/c/webots/gyro.h new file mode 100644 index 000000000..491b9a55e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/gyro.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/inertial_unit.h b/webots_ros2_driver/webots/include/controller/c/webots/inertial_unit.h new file mode 100644 index 000000000..13339d0fa --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/inertial_unit.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/joystick.h b/webots_ros2_driver/webots/include/controller/c/webots/joystick.h new file mode 100644 index 000000000..5e850efac --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/joystick.h @@ -0,0 +1,57 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/keyboard.h b/webots_ros2_driver/webots/include/controller/c/webots/keyboard.h new file mode 100644 index 000000000..18d3f313e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/keyboard.h @@ -0,0 +1,61 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/led.h b/webots_ros2_driver/webots/include/controller/c/webots/led.h new file mode 100644 index 000000000..fb5eb77e3 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/led.h @@ -0,0 +1,38 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/lidar.h b/webots_ros2_driver/webots/include/controller/c/webots/lidar.h new file mode 100644 index 000000000..e443a5948 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/lidar.h @@ -0,0 +1,66 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/lidar_point.h b/webots_ros2_driver/webots/include/controller/c/webots/lidar_point.h new file mode 100644 index 000000000..48cdeaa67 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/lidar_point.h @@ -0,0 +1,32 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/*************************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/light_sensor.h b/webots_ros2_driver/webots/include/controller/c/webots/light_sensor.h new file mode 100644 index 000000000..4388da029 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/light_sensor.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/microphone.h b/webots_ros2_driver/webots/include/controller/c/webots/microphone.h new file mode 100644 index 000000000..72b0e5cad --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/microphone.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/motor.h b/webots_ros2_driver/webots/include/controller/c/webots/motor.h new file mode 100644 index 000000000..6aabcf088 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/motor.h @@ -0,0 +1,75 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/mouse.h b/webots_ros2_driver/webots/include/controller/c/webots/mouse.h new file mode 100644 index 000000000..cc5bd1003 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/mouse.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/mouse_state.h b/webots_ros2_driver/webots/include/controller/c/webots/mouse_state.h new file mode 100644 index 000000000..38c13a107 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/mouse_state.h @@ -0,0 +1,40 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**************************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/nodes.h b/webots_ros2_driver/webots/include/controller/c/webots/nodes.h new file mode 100644 index 000000000..5f5e8edcc --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/nodes.h @@ -0,0 +1,120 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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_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, + // 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/webots_ros2_driver/webots/include/controller/c/webots/pen.h b/webots_ros2_driver/webots/include/controller/c/webots/pen.h new file mode 100644 index 000000000..dfd1312dc --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/pen.h @@ -0,0 +1,38 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/default.h b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/default.h new file mode 100644 index 000000000..080d34449 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/default.h @@ -0,0 +1,41 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h new file mode 100644 index 000000000..2a24b13dd --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/generic_robot_window/generic.h @@ -0,0 +1,44 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/*************************************************************************************/ +/* Description: 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(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/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_window.h b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_window.h new file mode 100644 index 000000000..602e2e0d0 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_window.h @@ -0,0 +1,30 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_wwi.h b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_wwi.h new file mode 100644 index 000000000..2b2d8d8c4 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/plugins/robot_window/robot_wwi.h @@ -0,0 +1,36 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/include/controller/c/webots/position_sensor.h b/webots_ros2_driver/webots/include/controller/c/webots/position_sensor.h new file mode 100644 index 000000000..a5783e54d --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/position_sensor.h @@ -0,0 +1,47 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/radar.h b/webots_ros2_driver/webots/include/controller/c/webots/radar.h new file mode 100644 index 000000000..9d0e9a928 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/radar.h @@ -0,0 +1,53 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/radar_target.h b/webots_ros2_driver/webots/include/controller/c/webots/radar_target.h new file mode 100644 index 000000000..62fd5d282 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/radar_target.h @@ -0,0 +1,31 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**************************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/radio.h b/webots_ros2_driver/webots/include/controller/c/webots/radio.h new file mode 100644 index 000000000..d39a56752 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/radio.h @@ -0,0 +1,75 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/range_finder.h b/webots_ros2_driver/webots/include/controller/c/webots/range_finder.h new file mode 100644 index 000000000..78da65e31 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/range_finder.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/receiver.h b/webots_ros2_driver/webots/include/controller/c/webots/receiver.h new file mode 100644 index 000000000..6eea5ceaf --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/receiver.h @@ -0,0 +1,53 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/remote_control.h b/webots_ros2_driver/webots/include/controller/c/webots/remote_control.h new file mode 100644 index 000000000..f5f704649 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/remote_control.h @@ -0,0 +1,128 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/robot.h b/webots_ros2_driver/webots/include/controller/c/webots/robot.h new file mode 100644 index 000000000..d114f8af2 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/robot.h @@ -0,0 +1,133 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/skin.h b/webots_ros2_driver/webots/include/controller/c/webots/skin.h new file mode 100644 index 000000000..620e87fc3 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/skin.h @@ -0,0 +1,42 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/speaker.h b/webots_ros2_driver/webots/include/controller/c/webots/speaker.h new file mode 100644 index 000000000..944a2df0a --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/speaker.h @@ -0,0 +1,49 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h b/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h new file mode 100644 index 000000000..435797490 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/supervisor.h @@ -0,0 +1,236 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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_proto_field(WbNodeRef node, const char *field_name); +WbFieldRef wb_supervisor_node_get_proto_field_by_index(WbNodeRef node, int index); +int wb_supervisor_node_get_proto_number_of_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); + +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); + +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_point_tracking(WbNodeRef node, int sampling_period, bool include_descendants); +void wb_supervisor_node_disable_contact_point_tracking(WbNodeRef node, bool include_descendants); + +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); + +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 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/webots_ros2_driver/webots/include/controller/c/webots/touch_sensor.h b/webots_ros2_driver/webots/include/controller/c/webots/touch_sensor.h new file mode 100644 index 000000000..573b7846d --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/touch_sensor.h @@ -0,0 +1,50 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/types.h b/webots_ros2_driver/webots/include/controller/c/webots/types.h new file mode 100644 index 000000000..9fd8fffc6 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/types.h @@ -0,0 +1,82 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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 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/webots_ros2_driver/webots/include/controller/c/webots/utils/ansi_codes.h b/webots_ros2_driver/webots/include/controller/c/webots/utils/ansi_codes.h new file mode 100644 index 000000000..17e70f4c1 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/utils/ansi_codes.h @@ -0,0 +1,63 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/include/controller/c/webots/utils/motion.h b/webots_ros2_driver/webots/include/controller/c/webots/utils/motion.h new file mode 100644 index 000000000..d838a0565 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/utils/motion.h @@ -0,0 +1,47 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/utils/string.h b/webots_ros2_driver/webots/include/controller/c/webots/utils/string.h new file mode 100644 index 000000000..c322b5e1e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/utils/string.h @@ -0,0 +1,35 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/**********************************************************************************/ +/* Description: 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, char *before, char *after); + +#ifdef __cplusplus +} +#endif + +#endif // WBU_STRING_H diff --git a/webots_ros2_driver/webots/include/controller/c/webots/utils/system.h b/webots_ros2_driver/webots/include/controller/c/webots/utils/system.h new file mode 100644 index 000000000..47abc0f40 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/utils/system.h @@ -0,0 +1,55 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/************************************************************************************/ +/* Description: 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/webots_ros2_driver/webots/include/controller/c/webots/vehicle/car.h b/webots_ros2_driver/webots/include/controller/c/webots/vehicle/car.h new file mode 100644 index 000000000..8822d270b --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/vehicle/car.h @@ -0,0 +1,87 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * Description: 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/webots_ros2_driver/webots/include/controller/c/webots/vehicle/driver.h b/webots_ros2_driver/webots/include/controller/c/webots/vehicle/driver.h new file mode 100644 index 000000000..9b97c13c9 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/c/webots/vehicle/driver.h @@ -0,0 +1,97 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * Description: 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/webots_ros2_driver/webots/include/controller/cpp/webots/Accelerometer.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Accelerometer.hpp new file mode 100644 index 000000000..8c89b7cc6 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Accelerometer.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Altimeter.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Altimeter.hpp new file mode 100644 index 000000000..2be38aedf --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Altimeter.hpp @@ -0,0 +1,32 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + virtual ~Altimeter() {} + virtual void enable(int samplingPeriod); + virtual void disable(); + int getSamplingPeriod() const; + double getValue() const; + }; +} // namespace webots + +#endif // ALTIMETER_HPP diff --git a/webots_ros2_driver/webots/include/controller/cpp/webots/Brake.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Brake.hpp new file mode 100644 index 000000000..b1e87e17f --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Brake.hpp @@ -0,0 +1,54 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Camera.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Camera.hpp new file mode 100644 index 000000000..1a43cf964 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Camera.hpp @@ -0,0 +1,67 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Compass.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Compass.hpp new file mode 100644 index 000000000..e737486f8 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Compass.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Connector.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Connector.hpp new file mode 100644 index 000000000..f624db67c --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Connector.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Device.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Device.hpp new file mode 100644 index 000000000..b9b884d04 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Device.hpp @@ -0,0 +1,42 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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); + + private: + WbDeviceTag tag; + std::string name; + }; +} // namespace webots + +#endif // DEVICE_HPP diff --git a/webots_ros2_driver/webots/include/controller/cpp/webots/Display.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Display.hpp new file mode 100644 index 000000000..f41cb555d --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Display.hpp @@ -0,0 +1,53 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/DistanceSensor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/DistanceSensor.hpp new file mode 100644 index 000000000..52f9b333f --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/DistanceSensor.hpp @@ -0,0 +1,40 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Emitter.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Emitter.hpp new file mode 100644 index 000000000..5e199cb3e --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Emitter.hpp @@ -0,0 +1,35 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp new file mode 100644 index 000000000..18cc39b41 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Field.hpp @@ -0,0 +1,127 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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; + + 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/webots_ros2_driver/webots/include/controller/cpp/webots/GPS.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/GPS.hpp new file mode 100644 index 000000000..eec05623b --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/GPS.hpp @@ -0,0 +1,42 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Gyro.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Gyro.hpp new file mode 100644 index 000000000..235b6da28 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Gyro.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/ImageRef.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/ImageRef.hpp new file mode 100644 index 000000000..2991065ff --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/ImageRef.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/InertialUnit.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/InertialUnit.hpp new file mode 100644 index 000000000..c76111729 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/InertialUnit.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Joystick.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Joystick.hpp new file mode 100644 index 000000000..c7b593441 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Joystick.hpp @@ -0,0 +1,43 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/Keyboard.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Keyboard.hpp new file mode 100644 index 000000000..fabf2dc26 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Keyboard.hpp @@ -0,0 +1,51 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/LED.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/LED.hpp new file mode 100644 index 000000000..f49d9565c --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/LED.hpp @@ -0,0 +1,30 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + virtual ~LED() {} + virtual void set(int value); + int get() const; + }; +} // namespace webots + +#endif // LED_HPP diff --git a/webots_ros2_driver/webots/include/controller/cpp/webots/Lidar.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Lidar.hpp new file mode 100644 index 000000000..ffb98911b --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Lidar.hpp @@ -0,0 +1,51 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/LightSensor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/LightSensor.hpp new file mode 100644 index 000000000..d2102bda4 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/LightSensor.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Motor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Motor.hpp new file mode 100644 index 000000000..f5a75318c --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Motor.hpp @@ -0,0 +1,83 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Mouse.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Mouse.hpp new file mode 100644 index 000000000..2f8f7b995 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Mouse.hpp @@ -0,0 +1,37 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp new file mode 100644 index 000000000..8cb3be9e6 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Node.hpp @@ -0,0 +1,190 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef NODE_HPP +#define NODE_HPP + +#define WB_USING_CPP_API +#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 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, + 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, + // 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; + Node *getFromProtoDef(const std::string &name) const; + int getNumberOfFields() const; + int getProtoNumberOfFields() const; + Field *getField(const std::string &fieldName) const; + Field *getProtoField(const std::string &fieldName) const; + Field *getFieldByIndex(const int index) const; + Field *getProtoFieldByIndex(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) const; + void disableContactPointsTracking() const; + void enableContactPointsTracking(int samplingPeriod, bool includeDescendants) const; + void disableContactPointsTracking(bool includeDescendants) 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/webots_ros2_driver/webots/include/controller/cpp/webots/Pen.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Pen.hpp new file mode 100644 index 000000000..0c3388f95 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Pen.hpp @@ -0,0 +1,30 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + virtual ~Pen() {} + virtual void write(bool write); + virtual void setInkColor(int color, double density); + }; +} // namespace webots + +#endif // PEN_HPP diff --git a/webots_ros2_driver/webots/include/controller/cpp/webots/PositionSensor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/PositionSensor.hpp new file mode 100644 index 000000000..51aaeeeb6 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/PositionSensor.hpp @@ -0,0 +1,56 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Radar.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Radar.hpp new file mode 100644 index 000000000..c33253d12 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Radar.hpp @@ -0,0 +1,40 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/RangeFinder.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/RangeFinder.hpp new file mode 100644 index 000000000..cebf3c743 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/RangeFinder.hpp @@ -0,0 +1,38 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Receiver.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Receiver.hpp new file mode 100644 index 000000000..e8c241659 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Receiver.hpp @@ -0,0 +1,40 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Robot.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Robot.hpp new file mode 100644 index 000000000..90b3ab846 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Robot.hpp @@ -0,0 +1,176 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef ROBOT_HPP +#define ROBOT_HPP + +#include +#include + +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 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); + + 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(const std::string &name) const; + virtual Altimeter *createAltimeter(const std::string &name) const; + virtual Brake *createBrake(const std::string &name) const; + virtual Camera *createCamera(const std::string &name) const; + virtual Compass *createCompass(const std::string &name) const; + virtual Connector *createConnector(const std::string &name) const; + virtual Display *createDisplay(const std::string &name) const; + virtual DistanceSensor *createDistanceSensor(const std::string &name) const; + virtual Emitter *createEmitter(const std::string &name) const; + virtual GPS *createGPS(const std::string &name) const; + virtual Gyro *createGyro(const std::string &name) const; + virtual InertialUnit *createInertialUnit(const std::string &name) const; + virtual LED *createLED(const std::string &name) const; + virtual Lidar *createLidar(const std::string &name) const; + virtual LightSensor *createLightSensor(const std::string &name) const; + virtual Motor *createMotor(const std::string &name) const; + virtual Pen *createPen(const std::string &name) const; + virtual PositionSensor *createPositionSensor(const std::string &name) const; + virtual Radar *createRadar(const std::string &name) const; + virtual RangeFinder *createRangeFinder(const std::string &name) const; + virtual Receiver *createReceiver(const std::string &name) const; + virtual Skin *createSkin(const std::string &name) const; + virtual Speaker *createSpeaker(const std::string &name) const; + virtual TouchSensor *createTouchSensor(const std::string &name) const; + + private: + Keyboard *mKeyboard; + Joystick *mJoystick; + Mouse *mMouse; + Device *getOrCreateDevice(int tag); + static std::vector deviceList; + }; +} // namespace webots + +#endif // ROBOT_HPP diff --git a/webots_ros2_driver/webots/include/controller/cpp/webots/Skin.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Skin.hpp new file mode 100644 index 000000000..c49978083 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Skin.hpp @@ -0,0 +1,34 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SKIN_HPP +#define SKIN_HPP + +#include + +namespace webots { + class Skin : public Device { + public: + explicit Skin(const std::string &name) : Device(name) {} + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Speaker.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Speaker.hpp new file mode 100644 index 000000000..88a2d9f4d --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Speaker.hpp @@ -0,0 +1,38 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/Supervisor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/Supervisor.hpp new file mode 100644 index 000000000..ac8bb9441 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/Supervisor.hpp @@ -0,0 +1,90 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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, 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/webots_ros2_driver/webots/include/controller/cpp/webots/TouchSensor.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/TouchSensor.hpp new file mode 100644 index 000000000..f7a0f6bd7 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/TouchSensor.hpp @@ -0,0 +1,42 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 + 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/webots_ros2_driver/webots/include/controller/cpp/webots/utils/AnsiCodes.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/utils/AnsiCodes.hpp new file mode 100644 index 000000000..b7cf6efd6 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/utils/AnsiCodes.hpp @@ -0,0 +1,47 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/utils/Motion.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/utils/Motion.hpp new file mode 100644 index 000000000..1b3a6fb18 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/utils/Motion.hpp @@ -0,0 +1,45 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Car.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Car.hpp new file mode 100644 index 000000000..02b40d376 --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Car.hpp @@ -0,0 +1,65 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Description: 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/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Driver.hpp b/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Driver.hpp new file mode 100644 index 000000000..12f93fa2d --- /dev/null +++ b/webots_ros2_driver/webots/include/controller/cpp/webots/vehicle/Driver.hpp @@ -0,0 +1,92 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Description: 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/webots_ros2_driver/webots/include/plugins/physics.h b/webots_ros2_driver/webots/include/plugins/physics.h new file mode 100644 index 000000000..56ef224e6 --- /dev/null +++ b/webots_ros2_driver/webots/include/plugins/physics.h @@ -0,0 +1,76 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef PHYSICS_H +#define PHYSICS_H +#include +#include +#ifdef _WIN32 +#include +#endif + +/* callback functions to be implemented in your physics plugin */ +#ifdef __cplusplus +extern "C" { +#endif +#ifdef _MSC_VER +#define DLLEXPORT __declspec(dllexport) +#else +#define DLLEXPORT +#endif +DLLEXPORT void webots_physics_init(); +DLLEXPORT int webots_physics_collide(dGeomID,dGeomID); +DLLEXPORT void webots_physics_step(); +DLLEXPORT void webots_physics_step_end(); +DLLEXPORT void webots_physics_cleanup(); +DLLEXPORT void webots_physics_draw(int pass, const char *view); + +/* utility functions to be used in your callback functions */ +#ifndef __linux__ +extern dGeomID (*dWebotsGetGeomFromDEFProc)(const char *); +extern dBodyID (*dWebotsGetBodyFromDEFProc)(const char *); +extern dJointGroupID (*dWebotsGetContactJointGroupProc)(); +extern void (*dWebotsSendProc)(int,const void *,int); +extern void* (*dWebotsReceiveProc)(int *); +extern void (*dWebotsConsolePrintfProc)(const char *, ...); +extern double (*dWebotsGetTimeProc)(); +#define dWebotsGetGeomFromDEF(defName) (*dWebotsGetGeomFromDEFProc)(defName) +#define dWebotsGetBodyFromDEF(defName) (*dWebotsGetBodyFromDEFProc)(defName) +#define dWebotsGetContactJointGroup() (*dWebotsGetContactJointGroupProc)() +#define dWebotsSend(channel,buff,size) (*dWebotsSendProc)(channel,buff,size) +#define dWebotsReceive(size_ptr) (*dWebotsReceiveProc)(size_ptr) + +#if defined(__VISUALC__) || defined (_MSC_VER) || defined(__BORLANDC__) +#define dWebotsConsolePrintf(format, ...) (*dWebotsConsolePrintfProc)(format, __VA_ARGS__) +#else +#define dWebotsConsolePrintf(format, ...) (*dWebotsConsolePrintfProc)(format, ## __VA_ARGS__) +#endif + +#define dWebotsGetTime() (*dWebotsGetTimeProc)() +#else +dGeomID dWebotsGetGeomFromDEF(const char *defName); +dBodyID dWebotsGetBodyFromDEF(const char *defName); +dJointGroupID dWebotsGetContactJointGroup(); +void dWebotsSend(int channel,const void *buffer,int size); +const void *dWebotsReceive(int *size); +void dWebotsConsolePrintf(const char *format, ...); +double dWebotsGetTime(); +#endif +#ifdef __cplusplus +} +#endif + +#endif /* PHYSICS_H */ diff --git a/webots_ros2_driver/webots/include/plugins/radio.h b/webots_ros2_driver/webots/include/plugins/radio.h new file mode 100644 index 000000000..2f0532a73 --- /dev/null +++ b/webots_ros2_driver/webots/include/plugins/radio.h @@ -0,0 +1,82 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef WEBOTS_RADIO_H +#define WEBOTS_RADIO_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* event types for WebotsRadioEvent */ +#define WEBOTS_RADIO_EVENT_RECEIVE 1 +#define WEBOTS_RADIO_EVENT_NETWORK_DETECTED 2 +#define WEBOTS_RADIO_EVENT_LOST_CONNECTION 3 + +struct WebotsRadioEvent { + int type; /* see above the WEBOTS_RADIO_EVENT_* type list */ + const char *data; /* data chunk received by the radio */ + int data_size; /* size of the data chunk */ + const char *from; /* address of the emitter of the data */ + double rssi; /* wireless networking parameter */ + void *user_data; /* user pointer defined in radio_set_callback() */ +}; + +void webots_radio_init(void); /* initialization routine, to be called + prior to any function call */ + +void webots_radio_cleanup(void); /* cleanup rountine, no further + webots_radio_xxx() function can be + called after this one */ + +int webots_radio_new(void); /* create a new radio node */ + +/* parameter setters */ +void webots_radio_set_protocol(int radio,const char *protocol); +void webots_radio_set_address(int radio,const char *address); +void webots_radio_set_frequency(int radio,double frequency); +void webots_radio_set_channel(int radio,int channel); +void webots_radio_set_bitrate(int radio,double bitrate); +void webots_radio_set_rx_sensitivity(int radio,double rx_sensitivity); +void webots_radio_set_tx_power(int radio,double tx_power); +void webots_radio_set_callback(int radio,void *user_data, + void(*)(const int radio,const struct WebotsRadioEvent *)); + +/* move a radio node */ +void webots_radio_move(int radio,double x,double y,double z); + +void webots_radio_send(int radio,const void *dest,const char *data,int size,double delay); + +void webots_radio_delete(int radio); /* delete a radio node */ + +void webots_radio_run(double seconds); /* run the network simulation for + a given number of seconds */ + +/* radio connected mode: to be implemented later on */ + +int webots_radio_connection_open(int radio,const char *destination); +int webots_radio_connection_send(int connection,const char *data,int size); +void webots_radio_connection_set_callback(int radio,void *ptr, + void(*receive_func)(void *ptr, + char *data, + int size)); +void webots_radio_connection_close(int connection); + +#ifdef __cplusplus +} +#endif + +#endif /* WEBOTS_RADIO_H */ diff --git a/webots_ros2_driver/webots/lib/controller/.gitignore b/webots_ros2_driver/webots/lib/controller/.gitignore new file mode 100644 index 000000000..259d2720d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/.gitignore @@ -0,0 +1,13 @@ +/java +/python34 +/python35 +/python36 +/python37 +/python38 +/python39 +/python310 +/python37_brew +/python38_brew +/python39_brew +/python310_brew +/*.so.* diff --git a/webots_ros2_driver/webots/lib/controller/matlab/.gitignore b/webots_ros2_driver/webots/lib/controller/matlab/.gitignore new file mode 100644 index 000000000..29d4cf19d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/.gitignore @@ -0,0 +1,535 @@ +wb_accelerometer_disable.m +wb_accelerometer_enable.m +wb_accelerometer_get_sampling_period.m +wb_altimeter_disable.m +wb_altimeter_enable.m +wb_altimeter_get_value.m +wb_altimeter_get_sampling_period.m +wb_brake_get_motor.m +wb_brake_get_position_sensor.m +wb_brake_get_type.m +wb_brake_set_damping_constant.m +wb_camera_disable.m +wb_camera_enable.m +wb_camera_get_exposure.m +wb_camera_get_focal_distance.m +wb_camera_get_focal_length.m +wb_camera_get_fov.m +wb_camera_get_height.m +wb_camera_get_max_focal_distance.m +wb_camera_get_max_fov.m +wb_camera_get_min_focal_distance.m +wb_camera_get_min_fov.m +wb_camera_get_near.m +wb_camera_get_sampling_period.m +wb_camera_get_width.m +wb_camera_has_recognition.m +wb_camera_recognition_disable.m +wb_camera_recognition_disable_segmentation.m +wb_camera_recognition_enable.m +wb_camera_recognition_enable_segmentation.m +wb_camera_recognition_get_number_of_objects.m +wb_camera_recognition_get_sampling_period.m +wb_camera_recognition_has_segmentation.m +wb_camera_recognition_is_segmentation_enabled.m +wb_camera_recognition_save_segmentation_image.m +wb_camera_save_image.m +wb_camera_set_exposure.m +wb_camera_set_focal_distance.m +wb_camera_set_fov.m +wb_compass_disable.m +wb_compass_enable.m +wb_compass_get_sampling_period.m +wb_connector_disable_presence.m +wb_connector_enable_presence.m +wb_connector_get_presence.m +wb_connector_get_presence_sampling_period.m +wb_connector_is_locked.m +wb_connector_lock.m +wb_connector_unlock.m +wb_console_print.m +wb_display_attach_camera.m +wb_display_detach_camera.m +wb_display_draw_line.m +wb_display_draw_oval.m +wb_display_draw_pixel.m +wb_display_draw_rectangle.m +wb_display_draw_text.m +wb_display_fill_oval.m +wb_display_fill_rectangle.m +wb_display_get_height.m +wb_display_get_width.m +wb_display_image_copy.m +wb_display_image_delete.m +wb_display_image_load.m +wb_display_image_paste.m +wb_display_image_save.m +wb_display_set_alpha.m +wb_display_set_font.m +wb_display_set_opacity.m +wb_device_get_model.m +wb_device_get_name.m +wb_device_get_node_type.m +wb_distance_sensor_disable.m +wb_distance_sensor_enable.m +wb_distance_sensor_get_aperture.m +wb_distance_sensor_get_max_value.m +wb_distance_sensor_get_min_value.m +wb_distance_sensor_get_sampling_period.m +wb_distance_sensor_get_type.m +wb_distance_sensor_get_value.m +wb_emitter_get_buffer_size.m +wb_emitter_get_channel.m +wb_emitter_get_range.m +wb_emitter_set_channel.m +wb_emitter_set_range.m +wb_gps_convert_to_degrees_minutes_seconds.m +wb_gps_disable.m +wb_gps_enable.m +wb_gps_get_coordinate_system.m +wb_gps_get_sampling_period.m +wb_gps_get_speed.m +wb_gyro_disable.m +wb_gyro_enable.m +wb_gyro_get_sampling_period.m +wb_inertial_unit_disable.m +wb_inertial_unit_enable.m +wb_inertial_unit_get_noise.m +wb_inertial_unit_get_sampling_period.m +wb_joystick_disable.m +wb_joystick_enable.m +wb_joystick_get_axis_value.m +wb_joystick_get_model.m +wb_joystick_get_number_of_axes.m +wb_joystick_get_number_of_povs.m +wb_joystick_get_pov_value.m +wb_joystick_get_pressed_button.m +wb_joystick_get_sampling_period.m +wb_joystick_is_connected.m +wb_joystick_set_auto_centering_gain.m +wb_joystick_set_constant_force.m +wb_joystick_set_constant_force_duration.m +wb_joystick_set_force_axis.m +wb_joystick_set_resistance_gain.m +wb_keyboard_disable.m +wb_keyboard_enable.m +wb_keyboard_get_key.m +wb_keyboard_get_sampling_period.m +wb_led_get.m +wb_led_set.m +wb_lidar_disable.m +wb_lidar_disable_point_cloud.m +wb_lidar_enable.m +wb_lidar_enable_point_cloud.m +wb_lidar_get_fov.m +wb_lidar_get_frequency.m +wb_lidar_get_horizontal_resolution.m +wb_lidar_get_max_frequency.m +wb_lidar_get_max_range.m +wb_lidar_get_min_frequency.m +wb_lidar_get_min_range.m +wb_lidar_get_number_of_layers.m +wb_lidar_get_number_of_points.m +wb_lidar_get_sampling_period.m +wb_lidar_get_vertical_fov.m +wb_lidar_is_point_cloud_enabled.m +wb_lidar_set_frequency.m +wb_light_sensor_disable.m +wb_light_sensor_enable.m +wb_light_sensor_get_sampling_period.m +wb_light_sensor_get_value.m +wb_motor_disable_force_feedback.m +wb_motor_disable_torque_feedback.m +wb_motor_enable_force_feedback.m +wb_motor_enable_torque_feedback.m +wb_motor_get_acceleration.m +wb_motor_get_available_force.m +wb_motor_get_available_torque.m +wb_motor_get_brake.m +wb_motor_get_force_feedback.m +wb_motor_get_force_feedback_sampling_period.m +wb_motor_get_max_force.m +wb_motor_get_max_position.m +wb_motor_get_max_torque.m +wb_motor_get_max_velocity.m +wb_motor_get_min_position.m +wb_motor_get_multiplier.m +wb_motor_get_position_sensor.m +wb_motor_get_target_position.m +wb_motor_get_torque_feedback.m +wb_motor_get_torque_feedback_sampling_period.m +wb_motor_get_type.m +wb_motor_get_velocity.m +wb_motor_set_acceleration.m +wb_motor_set_available_force.m +wb_motor_set_available_torque.m +wb_motor_set_control_pid.m +wb_motor_set_force.m +wb_motor_set_position.m +wb_motor_set_torque.m +wb_motor_set_velocity.m +wb_mouse_disable.m +wb_mouse_disable_3d_position.m +wb_mouse_enable.m +wb_mouse_enable_3d_position.m +wb_mouse_get_sampling_period.m +wb_mouse_is_3d_position_enabled.m +wb_pen_write.m +wb_position_sensor_disable.m +wb_position_sensor_enable.m +wb_position_sensor_get_brake.m +wb_position_sensor_get_motor.m +wb_position_sensor_get_sampling_period.m +wb_position_sensor_get_type.m +wb_position_sensor_get_value.m +wb_radar_disable.m +wb_radar_enable.m +wb_radar_get_horizontal_fov.m +wb_radar_get_max_range.m +wb_radar_get_min_range.m +wb_radar_get_number_of_targets.m +wb_radar_get_vertical_fov.m +wb_radar_get_sampling_period.m +wb_range_finder_disable.m +wb_range_finder_enable.m +wb_range_finder_get_fov.m +wb_range_finder_get_height.m +wb_range_finder_get_max_range.m +wb_range_finder_get_min_range.m +wb_range_finder_get_sampling_period.m +wb_range_finder_get_width.m +wb_range_finder_save_image.m +wb_receiver_disable.m +wb_receiver_enable.m +wb_receiver_get_channel.m +wb_receiver_get_data_size.m +wb_receiver_get_queue_length.m +wb_receiver_get_sampling_period.m +wb_receiver_get_signal_strength.m +wb_receiver_next_packet.m +wb_receiver_set_channel.m +wb_robot_step.m +wb_robot_step_begin.m +wb_robot_step_end.m +wb_robot_wait_for_user_input_event.m +wb_robot_battery_sensor_enable.m +wb_robot_battery_sensor_disable.m +wb_robot_battery_sensor_enable.m +wb_robot_battery_sensor_get_sampling_period.m +wb_robot_battery_sensor_get_value.m +wb_robot_get_basic_time_step.m +wb_robot_get_custom_data.m +wb_robot_get_data.m +wb_robot_get_device.m +wb_robot_get_device_by_index.m +wb_robot_get_mode.m +wb_robot_get_name.m +wb_robot_get_number_of_devices.m +wb_robot_get_project_path.m +wb_robot_get_supervisor.m +wb_robot_get_synchronization.m +wb_robot_get_time.m +wb_robot_get_urdf.m +wb_robot_get_world_path.m +wb_robot_set_custom_data.m +wb_robot_set_data.m +wb_robot_set_mode.m +wb_robot_step.m +wb_robot_wait_for_user_input_event.m +wb_robot_wwi_receive_text.m +wb_robot_wwi_send_text.m +wb_skin_get_bone_count.m +wb_skin_get_bone_name.m +wb_speaker_get_engine.m +wb_speaker_get_language.m +wb_speaker_is_sound_playing.m +wb_speaker_is_speaking.m +wb_speaker_play_sound.m +wb_speaker_set_engine.m +wb_speaker_set_language.m +wb_speaker_speak.m +wb_speaker_stop.m +wb_supervisor_animation_start_recording.m +wb_supervisor_animation_stop_recording.m +wb_supervisor_export_image.m +wb_supervisor_field_disable_sf_tracking.m +wb_supervisor_field_enable_sf_tracking.m +wb_supervisor_field_get_count.m +wb_supervisor_field_get_mf_bool.m +wb_supervisor_field_get_mf_float.m +wb_supervisor_field_get_mf_int32.m +wb_supervisor_field_get_mf_node.m +wb_supervisor_field_get_mf_string.m +wb_supervisor_field_get_name.m +wb_supervisor_field_get_sf_bool.m +wb_supervisor_field_get_sf_float.m +wb_supervisor_field_get_sf_int32.m +wb_supervisor_field_get_sf_node.m +wb_supervisor_field_get_sf_string.m +wb_supervisor_field_get_type.m +wb_supervisor_field_get_type_name.m +wb_supervisor_field_import_mf_node.m +wb_supervisor_field_import_mf_node_from_string.m +wb_supervisor_field_import_sf_node.m +wb_supervisor_field_import_sf_node_from_string.m +wb_supervisor_field_insert_mf_bool.m +wb_supervisor_field_insert_mf_float.m +wb_supervisor_field_insert_mf_int32.m +wb_supervisor_field_insert_mf_string.m +wb_supervisor_field_remove_mf.m +wb_supervisor_field_remove_mf_node.m +wb_supervisor_field_remove_sf.m +wb_supervisor_field_set_mf_bool.m +wb_supervisor_field_set_mf_float.m +wb_supervisor_field_set_mf_int32.m +wb_supervisor_field_set_mf_string.m +wb_supervisor_field_set_sf_bool.m +wb_supervisor_field_set_sf_float.m +wb_supervisor_field_set_sf_int32.m +wb_supervisor_field_set_sf_string.m +wb_supervisor_movie_failed.m +wb_supervisor_movie_is_ready.m +wb_supervisor_movie_start_recording.m +wb_supervisor_movie_stop_recording.m +wb_supervisor_node_add_force.m +wb_supervisor_node_add_force_with_offset.m +wb_supervisor_node_add_torque.m +wb_supervisor_node_disable_contact_point_tracking.m +wb_supervisor_node_disable_pose_tracking.m +wb_supervisor_node_enable_contact_point_tracking.m +wb_supervisor_node_enable_pose_tracking.m +wb_supervisor_node_export_string.m +wb_supervisor_node_get_base_type_name.m +wb_supervisor_node_get_contact_point_node.m +wb_supervisor_node_get_def.m +wb_supervisor_node_get_field.m +wb_supervisor_node_get_field_by_index.m +wb_supervisor_node_get_from_def.m +wb_supervisor_node_get_from_device.m +wb_supervisor_node_get_from_id.m +wb_supervisor_node_get_from_proto_def.m +wb_supervisor_node_get_id.m +wb_supervisor_node_get_number_of_contact_points.m +wb_supervisor_node_get_number_of_fields.m +wb_supervisor_node_get_parent_node.m +wb_supervisor_node_get_proto_field.m +wb_supervisor_node_get_proto_field_by_index.m +wb_supervisor_node_get_proto_number_of_fields.m +wb_supervisor_node_get_root.m +wb_supervisor_node_get_selected.m +wb_supervisor_node_get_self.m +wb_supervisor_node_get_static_balance.m +wb_supervisor_node_get_type.m +wb_supervisor_node_get_type_name.m +wb_supervisor_node_is_proto.m +wb_supervisor_node_load_state.m +wb_supervisor_node_move_viewpoint.m +wb_supervisor_node_remove.m +wb_supervisor_node_reset_physics.m +wb_supervisor_node_restart_controller.m +wb_supervisor_node_save_state.m +wb_supervisor_node_set_joint_position.m +wb_supervisor_node_set_velocity.m +wb_supervisor_node_set_visibility.m +wb_supervisor_simulation_get_mode.m +wb_supervisor_simulation_quit.m +wb_supervisor_simulation_reset.m +wb_supervisor_simulation_reset_physics.m +wb_supervisor_simulation_set_mode.m +wb_supervisor_virtual_reality_headset_is_used.m +wb_supervisor_world_load.m +wb_supervisor_world_reload.m +wb_touch_sensor_disable.m +wb_touch_sensor_enable.m +wb_touch_sensor_get_sampling_period.m +wb_touch_sensor_get_type.m +wb_touch_sensor_get_value.m +wbu_motion_delete.m +wbu_motion_get_duration.m +wbu_motion_get_time.m +wbu_motion_is_over.m +wbu_motion_new.m +wbu_motion_play.m +wbu_motion_set_loop.m +wbu_motion_set_reverse.m +wbu_motion_set_time.m +wbu_motion_stop.m +wbu_system_getenv.m +wbu_system_short_path.m +WB_STDOUT.m +WB_STDERR.m +WB_CHANNEL_BROADCAST.m +WB_IMAGE_RGB.m +WB_IMAGE_RGBA.m +WB_IMAGE_ARGB.m +WB_IMAGE_BGRA.m +WB_IMAGE_ABGR.m +WB_KEYBOARD_KEY.m +WB_KEYBOARD_SHIFT.m +WB_KEYBOARD_CONTROL.m +WB_KEYBOARD_ALT.m +WB_KEYBOARD_LEFT.m +WB_KEYBOARD_UP.m +WB_KEYBOARD_RIGHT.m +WB_KEYBOARD_DOWN.m +WB_KEYBOARD_PAGEUP.m +WB_KEYBOARD_PAGEDOWN.m +WB_KEYBOARD_HOME.m +WB_KEYBOARD_END.m +WB_KEYBOARD_NUMPAD_UP.m +WB_KEYBOARD_NUMPAD_DOWN.m +WB_KEYBOARD_NUMPAD_LEFT.m +WB_KEYBOARD_NUMPAD_RIGHT.m +WB_KEYBOARD_NUMPAD_HOME.m +WB_KEYBOARD_NUMPAD_END.m +WB_NO_FIELD.m +WB_SF_BOOL.m +WB_SF_INT32.m +WB_SF_FLOAT.m +WB_SF_VEC2F.m +WB_SF_VEC3F.m +WB_SF_ROTATION.m +WB_SF_COLOR.m +WB_SF_STRING.m +WB_SF_NODE.m +WB_MF.m +WB_MF_BOOL.m +WB_MF_INT32.m +WB_MF_FLOAT.m +WB_MF_VEC2F.m +WB_MF_VEC3F.m +WB_MF_ROTATION.m +WB_MF_COLOR.m +WB_MF_STRING.m +WB_MF_NODE.m +WB_EVENT_QUIT.m +WB_EVENT_NO_EVENT.m +WB_EVENT_MOUSE_CLICK.m +WB_EVENT_MOUSE_MOVE.m +WB_EVENT_KEYBOARD.m +WB_EVENT_JOYSTICK_BUTTON.m +WB_EVENT_JOYSTICK_AXIS.m +WB_EVENT_JOYSTICK_POV.m +ANSI_RESET.m +ANSI_BOLD.m +ANSI_UNDERLINE.m +ANSI_BLACK_FOREGROUND.m +ANSI_RED_FOREGROUND.m +ANSI_GREEN_FOREGROUND.m +ANSI_YELLOW_FOREGROUND.m +ANSI_BLUE_FOREGROUND.m +ANSI_MAGENTA_FOREGROUND.m +ANSI_CYAN_FOREGROUND.m +ANSI_WHITE_FOREGROUND.m +ANSI_BLACK_BACKGROUND.m +ANSI_RED_BACKGROUND.m +ANSI_GREEN_BACKGROUND.m +ANSI_YELLOW_BACKGROUND.m +ANSI_BLUE_BACKGROUND.m +ANSI_MAGENTA_BACKGROUND.m +ANSI_CYAN_BACKGROUND.m +ANSI_WHITE_BACKGROUND.m +ANSI_CLEAR_SCREEN.m +WB_DISTANCE_SENSOR_GENERIC.m +WB_DISTANCE_SENSOR_INFRA_RED.m +WB_DISTANCE_SENSOR_SONAR.m +WB_DISTANCE_SENSOR_LASER.m +WB_GPS_LOCAL_COORDINATE.m +WB_GPS_WGS84_COORDINATE.m +WB_MODE_SIMULATION.m +WB_MODE_CROSS_COMPILATION.m +WB_MODE_REMOTE_CONTROL.m +WB_NODE_NO_NODE.m +WB_NODE_APPEARANCE.m +WB_NODE_BACKGROUND.m +WB_NODE_BILLBOARD.m +WB_NODE_BOX.m +WB_NODE_CAD_SHAPE.m +WB_NODE_CAPSULE.m +WB_NODE_COLOR.m +WB_NODE_CONE.m +WB_NODE_COORDINATE.m +WB_NODE_CYLINDER.m +WB_NODE_DIRECTIONAL_LIGHT.m +WB_NODE_ELEVATION_GRID.m +WB_NODE_FOG.m +WB_NODE_GROUP.m +WB_NODE_IMAGE_TEXTURE.m +WB_NODE_INDEXED_FACE_SET.m +WB_NODE_INDEXED_LINE_SET.m +WB_NODE_MATERIAL.m +WB_NODE_MESH.m +WB_NODE_MUSCLE.m +WB_NODE_NORMAL.m +WB_NODE_PBR_APPEARANCE.m +WB_NODE_PLANE.m +WB_NODE_POINT_LIGHT.m +WB_NODE_POINT_SET.m +WB_NODE_SHAPE.m +WB_NODE_SPHERE.m +WB_NODE_SPOT_LIGHT.m +WB_NODE_TEXTURE_COORDINATE.m +WB_NODE_TEXTURE_TRANSFORM.m +WB_NODE_TRANSFORM.m +WB_NODE_VIEWPOINT.m +WB_NODE_ROBOT.m +WB_NODE_ACCELEROMETER.m +WB_NODE_ALTIMETER.m +WB_NODE_BRAKE.m +WB_NODE_CAMERA.m +WB_NODE_COMPASS.m +WB_NODE_CONNECTOR.m +WB_NODE_DISPLAY.m +WB_NODE_DISTANCE_SENSOR.m +WB_NODE_EMITTER.m +WB_NODE_GPS.m +WB_NODE_GYRO.m +WB_NODE_INERTIAL_UNIT.m +WB_NODE_LED.m +WB_NODE_LIDAR.m +WB_NODE_LIGHT_SENSOR.m +WB_NODE_LINEAR_MOTOR.m +WB_NODE_PEN.m +WB_NODE_POSITION_SENSOR.m +WB_NODE_PROPELLER.m +WB_NODE_RADAR.m +WB_NODE_RANGE_FINDER.m +WB_NODE_RECEIVER.m +WB_NODE_ROTATIONAL_MOTOR.m +WB_NODE_SKIN.m +WB_NODE_SPEAKER.m +WB_NODE_TOUCH_SENSOR.m +WB_NODE_BALL_JOINT.m +WB_NODE_BALL_JOINT_PARAMETERS.m +WB_NODE_CHARGER.m +WB_NODE_CONTACT_PROPERTIES.m +WB_NODE_DAMPING.m +WB_NODE_FLUID.m +WB_NODE_FOCUS.m +WB_NODE_HINGE_JOINT.m +WB_NODE_HINGE_JOINT_PARAMETERS.m +WB_NODE_HINGE_2_JOINT.m +WB_NODE_IMMERSION_PROPERTIES.m +WB_NODE_JOINT_PARAMETERS.m +WB_NODE_LENS.m +WB_NODE_LENS_FLARE.m +WB_NODE_PHYSICS.m +WB_NODE_RECOGNITION.m +WB_NODE_SLIDER_JOINT.m +WB_NODE_SLOT.m +WB_NODE_SOLID.m +WB_NODE_SOLID_REFERENCE.m +WB_NODE_TRACK.m +WB_NODE_TRACK_WHEEL.m +WB_NODE_WORLD_INFO.m +WB_NODE_ZOOM.m +WB_NODE_MICROPHONE.m +WB_NODE_RADIO.m +WB_SUPERVISOR_SIMULATION_MODE_PAUSE.m +WB_SUPERVISOR_SIMULATION_MODE_REAL_TIME.m +WB_SUPERVISOR_SIMULATION_MODE_FAST.m +WB_ROTATIONAL.m +WB_LINEAR.m +WB_TOUCH_SENSOR_BUMPER.m +WB_TOUCH_SENSOR_FORCE.m +WB_TOUCH_SENSOR_FORCE3D.m diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_ANGULAR.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_ANGULAR.m new file mode 100644 index 000000000..84c7228ef --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_ANGULAR.m @@ -0,0 +1,2 @@ +function value = WB_ANGULAR +value = 0; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_ENCODING_ERROR.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_ENCODING_ERROR.m new file mode 100644 index 000000000..2b91057c5 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_ENCODING_ERROR.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_ENCODING_ERROR +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_ENCODING_ERROR is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 4; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_READY.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_READY.m new file mode 100644 index 000000000..7c47a2374 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_READY.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_READY +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_READY is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 0; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_RECORDING.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_RECORDING.m new file mode 100644 index 000000000..511e6118c --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_RECORDING.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_RECORDING +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_RECORDING is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 1; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SAVING.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SAVING.m new file mode 100644 index 000000000..7a6d55f75 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SAVING.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_SAVING +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_SAVING is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 2; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SIMULATION_ERROR.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SIMULATION_ERROR.m new file mode 100644 index 000000000..a4cd96fca --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_SIMULATION_ERROR.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_SIMULATION_ERROR +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_SIMULATION_ERROR is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 5; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_WRITE_ERROR.m b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_WRITE_ERROR.m new file mode 100644 index 000000000..4c1df1aba --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/WB_SUPERVISOR_MOVIE_WRITE_ERROR.m @@ -0,0 +1,4 @@ +function value = WB_SUPERVISOR_MOVIE_WRITE_ERROR +% DEPRECATED +warning('WB_SUPERVISOR_MOVIE_WRITE_ERROR is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); +value = 3; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/allincludes.h b/webots_ros2_driver/webots/lib/controller/matlab/allincludes.h new file mode 100644 index 000000000..83f782e63 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/allincludes.h @@ -0,0 +1,40 @@ +/* + This file is used by the loadlibrary() function in the launcher.m (MATLAB) file. + It contains all the headers files of Webots C-API. +*/ + +#define WB_MATLAB_LOADLIBRARY + +#include "../../../include/controller/c/webots/accelerometer.h" +#include "../../../include/controller/c/webots/altimeter.h" +#include "../../../include/controller/c/webots/brake.h" +#include "../../../include/controller/c/webots/camera.h" +#include "../../../include/controller/c/webots/compass.h" +#include "../../../include/controller/c/webots/connector.h" +#include "../../../include/controller/c/webots/console.h" +#include "../../../include/controller/c/webots/device.h" +#include "../../../include/controller/c/webots/display.h" +#include "../../../include/controller/c/webots/distance_sensor.h" +#include "../../../include/controller/c/webots/emitter.h" +#include "../../../include/controller/c/webots/gps.h" +#include "../../../include/controller/c/webots/gyro.h" +#include "../../../include/controller/c/webots/inertial_unit.h" +#include "../../../include/controller/c/webots/joystick.h" +#include "../../../include/controller/c/webots/keyboard.h" +#include "../../../include/controller/c/webots/led.h" +#include "../../../include/controller/c/webots/lidar.h" +#include "../../../include/controller/c/webots/light_sensor.h" +#include "../../../include/controller/c/webots/motor.h" +#include "../../../include/controller/c/webots/mouse.h" +#include "../../../include/controller/c/webots/pen.h" +#include "../../../include/controller/c/webots/position_sensor.h" +#include "../../../include/controller/c/webots/radar.h" +#include "../../../include/controller/c/webots/range_finder.h" +#include "../../../include/controller/c/webots/receiver.h" +#include "../../../include/controller/c/webots/robot.h" +#include "../../../include/controller/c/webots/skin.h" +#include "../../../include/controller/c/webots/speaker.h" +#include "../../../include/controller/c/webots/supervisor.h" +#include "../../../include/controller/c/webots/touch_sensor.h" +#include "../../../include/controller/c/webots/utils/motion.h" +#include "../../../include/controller/c/webots/utils/system.h" diff --git a/webots_ros2_driver/webots/lib/controller/matlab/finish.m b/webots_ros2_driver/webots/lib/controller/matlab/finish.m new file mode 100644 index 000000000..20cea1fc1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/finish.m @@ -0,0 +1,7 @@ +% Matlab API for Webots +% Cleans up and unloads libController before quitting Matlab + +if libisloaded('libController') + calllib('libController','wb_robot_cleanup'); + unloadlibrary('libController'); +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/launcher.m b/webots_ros2_driver/webots/lib/controller/matlab/launcher.m new file mode 100644 index 000000000..addf76031 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/launcher.m @@ -0,0 +1,192 @@ +% Launcher script for MATLAB Webots controllers + +% useful env variables supplied by webots +WEBOTS_HOME = getenv('WEBOTS_HOME'); +if ismac + WEBOTS_HOME_PATH = [ WEBOTS_HOME '/Contents' ]; +else + WEBOTS_HOME_PATH = WEBOTS_HOME; +end +WEBOTS_CONTROLLER_NAME = getenv('WEBOTS_CONTROLLER_NAME'); +WEBOTS_VERSION = getenv('WEBOTS_VERSION'); + +if isempty(WEBOTS_CONTROLLER_NAME) + disp('Entering test mode (normally launcher.m should be called by Webots, not from the MATLAB command line)'); + disp(['Using MATLAB R' version('-release')]); + if isempty(WEBOTS_HOME) + cd('../../..'); + WEBOTS_HOME_PATH = pwd; + else + cd(WEBOTS_HOME_PATH) + end + [status, cmdout] = system('msys64/mingw64/bin/webots.exe --version'); + k = strfind(cmdout, ' Nightly Build '); + if isempty(k) + WEBOTS_VERSION = strrep(cmdout(17:end-1),'.','_'); + else + WEBOTS_VERSION = strrep(cmdout(17:k(1)-1),'.','_'); + end + disp(['Using Webots ' strrep(WEBOTS_VERSION,'_','.') ' from ' pwd ]); + test_mode = true; +else + test_mode = false; +end + +% add path to Webots API m-files +addpath([WEBOTS_HOME_PATH '/lib/controller/matlab']); + +if ispc + setenv('MINGWROOT', strcat(WEBOTS_HOME,'\\msys64\\mingw64')); + libname = 'Controller'; + % in MATLAB 2017b, the following tests are crashing MATLAB + % in MATLAB 2019b, the MingGW-w64 C/C++ Compiler is not needed any more + if version('-release') == '2018b' + installed_addons = matlab.addons.installedAddons; + installed = sum(installed_addons.Identifier == "ML_MINGW"); + if installed <= 0 || matlab.addons.isAddonEnabled("ML_MINGW") <= 0 + disp('The MATLAB "MinGW-w64 C/C++ Compiler" addon is not installed, please install it from: https://fr.mathworks.com/matlabcentral/fileexchange/52848-matlab-support-for-mingw-w64-c-c-compiler'); + end + end + addpath([WEBOTS_HOME '/msys64/mingw64/bin']); +else + libname = 'libController'; + % add path to libController + addpath([WEBOTS_HOME_PATH '/lib/controller']); +end + +try + % work in the system's temp dir so we have write access + cd(tempdir); % that supports non-ASCII character since MATLAB 2016a + + % creates the base name of the protofile (without .m extension) + % we make the name dependant on matlab and webots versions, so a new file will be generated for any version change + % we need to replace the dots by underscores, because dots cause problems in the protofile + protofile = strrep(strrep(['protofile_matlab_' version('-release') '_webots_' WEBOTS_VERSION], '.', '_'), ' ', '_'); + + % another controller is currently generating the proto file: need to wait + lockfile = 'webots_matlab_lock'; + counter = 1; + while exist(lockfile, 'file') + if counter == 1 + disp(['Waiting up to 5 seconds for ' tempdir lockfile ' to be deleted by another MATLAB instance.']); + end + pause(1); + if counter == 5 + disp(['Deleting ' tempdir lockfile '...']) + delete(lockfile); + end + counter = counter + 1; + end + + libcontrollerloaded = false; + + if ~exist([protofile '.m'], 'file') + + % create a lock to prevent any other matlab controller from generating the proto file simultaneously + fid = fopen(lockfile, 'w'); + fclose(fid); + + disp(['Creating: ' tempdir protofile '.m']); + try + loadlibrary( ... + libname,'allincludes.h', ... + 'mfilename',protofile, ... + 'alias','libController', ... + 'addheader','accelerometer.h', ... + 'addheader','altimeter.h',... + 'addheader','brake.h', ... + 'addheader','camera.h', ... + 'addheader','compass.h', ... + 'addheader','connector.h', ... + 'addheader','console.h', ... + 'addheader','device.h', ... + 'addheader','display.h', ... + 'addheader','distance_sensor.h', ... + 'addheader','emitter.h', ... + 'addheader','gps.h', ... + 'addheader','gyro.h', ... + 'addheader','inertial_unit.h', ... + 'addheader','joystick.h', ... + 'addheader','keyboard.h', ... + 'addheader','led.h', ... + 'addheader','lidar.h', ... + 'addheader','light_sensor.h', ... + 'addheader','motor.h', ... + 'addheader','mouse.h', ... + 'addheader','pen.h', ... + 'addheader','position_sensor.h', ... + 'addheader','radar.h', ... + 'addheader','range_finder.h', ... + 'addheader','receiver.h', ... + 'addheader','robot.h', ... + 'addheader','skin.h', ... + 'addheader','speaker.h', ... + 'addheader','supervisor.h', ... + 'addheader','touch_sensor.h', ... + 'addheader',['utils' filesep 'motion.h'], ... + 'addheader',['utils' filesep 'system.h']); + disp('Load Library successful'); + libcontrollerloaded = true; + catch ME % this happens with MATLAB R2015a only but seems to be harmless + if version('-release') == '2015a' + loadlibrary(libname,protofile,'alias','libController'); + libcontrollerloaded = true; + else + disp('Load Library failed.'); + rethrow(ME); + loadlibrary = false; + end + end + delete(lockfile); + else + disp(['Using prototype file: ' tempdir protofile '.m']); + loadlibrary(libname,protofile,'alias','libController'); + libcontrollerloaded = true; + end + + if test_mode == true + unloadlibrary('libController'); + cd([WEBOTS_HOME_PATH '/lib/controller/matlab']); + disp('Test successful.'); + return + end + + % initialize libController and redirect stdout/stderr + try + calllib('libController', 'wb_robot_init'); + catch + calllib('libController', 'wb_robot_init_msvc'); + end + + % start controller + WEBOTS_PROJECT_UTF8 = wbu_system_getenv('WEBOTS_PROJECT'); + WEBOTS_PROJECT = wbu_system_short_path(WEBOTS_PROJECT_UTF8); + + cd([WEBOTS_PROJECT '/controllers/' WEBOTS_CONTROLLER_NAME]); + + % sanitize controller name if needed + if ~isvarname(WEBOTS_CONTROLLER_NAME) + newname = matlab.lang.makeValidName(WEBOTS_CONTROLLER_NAME); + copyfile(append(WEBOTS_CONTROLLER_NAME, '.m'), append(newname, '.m'), 'f'); + eval(newname); + delete(append(newname, '.m')); % delete temporary file + else + eval(WEBOTS_CONTROLLER_NAME); + end + +catch ME + % display error message in Webots console + % use stderr to display message in red (this does not work on Windows) + err = getReport(ME, 'extended'); + fprintf(2,'%s\n\n',err); + if ispc + if libcontrollerloaded + % only try to put the error on the console if the library has been loaded + calllib('libController', 'wb_console_print', err, 2); + exit(-1); + end + % on Windows, exiting systematically would imply to lose the error message + else + exit(-1); + end +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_lookup_table.m new file mode 100644 index 000000000..6cf3762cd --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_lookup_table.m @@ -0,0 +1,9 @@ +function wb_accelerometer_get_lookup_table(tag) +% Usage: wb_accelerometer_get_lookup_table(tag) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_accelerometer_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_accelerometer_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_values.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_values.m new file mode 100644 index 000000000..b2863bfe6 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_accelerometer_get_values.m @@ -0,0 +1,8 @@ +function result = wb_accelerometer_get_values(tag) +% Usage: wb_accelerometer_get_values(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_accelerometer_get_values', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_get_image.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_get_image.m new file mode 100644 index 000000000..9d0d621c0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_get_image.m @@ -0,0 +1,11 @@ +function rgb = wb_camera_get_image(tag) +% Usage: wb_camera_get_image(tag) +% Matlab API for Webots +% Online documentation is available here + +width = calllib('libController', 'wb_camera_get_width', tag); +height = calllib('libController', 'wb_camera_get_height', tag); +pointer = calllib('libController', 'wb_camera_get_image',tag); +setdatatype(pointer, 'uint8Ptr', 4 * width * height, 1); +rgba = permute(reshape(get(pointer, 'Value'), 4, width, height), [3 2 1]); +rgb(:,:,[1 2 3]) = rgba(:,:,[3 2 1]); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_objects.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_objects.m new file mode 100644 index 000000000..b53de72a0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_objects.m @@ -0,0 +1,16 @@ +function objects = wb_camera_recognition_get_objects(tag) +% Usage: wb_camera_recognition_get_objects(tag) +% Matlab API for Webots +% Online documentation is available here + +rate = calllib('libController', 'wb_camera_recognition_get_sampling_period', tag); +if rate <= 0 + targets = [] + calllib('libController', 'wb_console_print', 'Warning: wb_camera_recognition_get_objects() called for a disabled device! Please use: wb_camera_recognition_enable() before.', 2); +else + size = calllib('libController', 'wb_camera_recognition_get_number_of_objects', tag); + for n = 1:size + obj = calllib('libController', 'wb_camera_recognition_get_object', tag, n - 1); + targets(n) = obj.Value; + end +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_segmentation_image.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_segmentation_image.m new file mode 100644 index 000000000..d030d24db --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_camera_recognition_get_segmentation_image.m @@ -0,0 +1,11 @@ +function rgb = wb_camera_recognition_get_segmentation_image(tag) +% Usage: wb_camera_recognition_get_segmentation_image(tag) +% Matlab API for Webots +% Online documentation is available here + +width = calllib('libController', 'wb_camera_get_width', tag); +height = calllib('libController', 'wb_camera_get_height', tag); +pointer = calllib('libController', 'wb_camera_recognition_get_segmentation_image',tag); +setdatatype(pointer, 'uint8Ptr', 4 * width * height, 1); +rgba = permute(reshape(get(pointer, 'Value'), 4, width, height), [3 2 1]); +rgb(:,:,[1 2 3]) = rgba(:,:,[3 2 1]); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_lookup_table.m new file mode 100644 index 000000000..087ec429b --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_lookup_table.m @@ -0,0 +1,9 @@ +function wb_compass_get_lookup_table(tag, sampling_period) +% Usage: wb_compass_get_lookup_table(tag, sampling_period) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_compass_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_compass_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_values.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_values.m new file mode 100644 index 000000000..efc4774fa --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_compass_get_values.m @@ -0,0 +1,8 @@ +function result = wb_compass_get_values(tag) +% Usage: wb_compass_get_values(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_compass_get_values', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_display_draw_polygon.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_draw_polygon.m new file mode 100644 index 000000000..52cc109d4 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_draw_polygon.m @@ -0,0 +1,9 @@ +function wb_display_draw_polygon(tag, x, y) +% Usage: wb_display_draw_polygon(tag, x, y) +% Matlab API for Webots +% Online documentation is available here + +assert(~isvector(x), 'Invalid ''x'' argument: is not a vector'); +assert(~isvector(y), 'Invalid ''y'' argument: is not a vector'); +assert(numel(x) == numel(y), 'Invalid arguments: number of elements in ''x'' and ''y'' differ'); +calllib('libController', 'wb_display_draw_polygon', tag, x, y, numel(x)); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_display_fill_polygon.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_fill_polygon.m new file mode 100644 index 000000000..b02571fc8 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_fill_polygon.m @@ -0,0 +1,9 @@ +function wb_display_fill_polygon(tag, x, y) +% Usage: wb_display_fill_polygon(tag, x, y) +% Matlab API for Webots +% Online documentation is available here + +assert(~isvector(x), 'Invalid ''x'' argument: is not a vector'); +assert(~isvector(y), 'Invalid ''y'' argument: is not a vector'); +assert(numel(x) == numel(y), 'Invalid arguments: number of elements in ''x'' and ''y'' differ'); +calllib('libController', 'wb_display_fill_polygon', tag, x, y, numel(x)); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_display_image_new.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_image_new.m new file mode 100644 index 000000000..bebf896ee --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_image_new.m @@ -0,0 +1,12 @@ +function imageref = wb_display_image_new(tag,rgb,format) +% Usage: wb_camera_get_image(tag,rgb,format) +% Matlab API for Webots +% Online documentation is available here + +% need to stay in memory until wb_robot_step() +persistent pointer; + +height=size(rgb,1); +width=size(rgb,2); +pointer = libpointer('uint8Ptr',permute(rgb,[3 2 1])); +imageref = calllib('libController','wb_display_image_new',tag,width,height,pointer,format); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_display_set_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_set_color.m new file mode 100644 index 000000000..4c2b7583e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_display_set_color.m @@ -0,0 +1,8 @@ +function wb_display_set_color(tag, color) +% Usage: wb_display_set_color(tag, color) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(color) == 3, 'Invalid ''color'' argument: [red green blue] array expected'); +colorbytes = 65536 * 255 * color(1) + 256 * 255 * color(2) + 255 * color(3); +calllib('libController', 'wb_display_set_color', tag, colorbytes); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_distance_sensor_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_distance_sensor_get_lookup_table.m new file mode 100644 index 000000000..20474f64d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_distance_sensor_get_lookup_table.m @@ -0,0 +1,9 @@ +function result = wb_distance_sensor_get_lookup_table(tag) +% Usage: wb_distance_sensor_get_lookup_table(tag) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_distance_sensor_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_distance_sensor_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_emitter_send.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_emitter_send.m new file mode 100644 index 000000000..6e2f7ad78 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_emitter_send.m @@ -0,0 +1,7 @@ +function result = wb_emitter_send(tag, data) +% Usage: wb_emitter_send(tag, data) +% Matlab API for Webots +% Online documentation is available here + +w=whos('data'); +result = calllib('libController', 'wb_emitter_send', tag, data, w.bytes); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_speed_vector.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_speed_vector.m new file mode 100644 index 000000000..a6712e248 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_speed_vector.m @@ -0,0 +1,8 @@ +function result = wb_gps_get_speed_vector(tag) +% Usage: wb_gps_get_speed_vector(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_gps_get_speed_vector', tag); +setdatatype(obj, 'doublePtr', 1, 3); +result = get(obj, 'Value')'; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_values.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_values.m new file mode 100644 index 000000000..615b7b855 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_gps_get_values.m @@ -0,0 +1,8 @@ +function result = wb_gps_get_values(tag) +% Usage: wb_gps_get_values(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_gps_get_values', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_lookup_table.m new file mode 100644 index 000000000..153a2a465 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_lookup_table.m @@ -0,0 +1,9 @@ +function wb_gyro_get_lookup_table(tag, sampling_period) +% Usage: wb_gyro_get_lookup_table(tag, sampling_period) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_gyro_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_gyro_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_values.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_values.m new file mode 100644 index 000000000..41033dff9 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_gyro_get_values.m @@ -0,0 +1,8 @@ +function result = wb_gyro_get_values(tag) +% Usage: wb_gyro_get_values(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_gyro_get_values', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_quaternion.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_quaternion.m new file mode 100644 index 000000000..e64c5d21b --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_quaternion.m @@ -0,0 +1,8 @@ +function result = wb_inertial_unit_get_quaternion(tag) +% Usage: wb_inertial_unit_get_quaternion(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_inertial_unit_get_quaternion', tag); +setdatatype(obj,'doublePtr', 1, 4); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_roll_pitch_yaw.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_roll_pitch_yaw.m new file mode 100644 index 000000000..c5b98b1b5 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_inertial_unit_get_roll_pitch_yaw.m @@ -0,0 +1,8 @@ +function result = wb_inertial_unit_get_roll_pitch_yaw(tag) +% Usage: wb_inertial_unit_get_roll_pitch_yaw(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_inertial_unit_get_roll_pitch_yaw', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_point_cloud.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_point_cloud.m new file mode 100644 index 000000000..e4b992353 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_point_cloud.m @@ -0,0 +1,10 @@ +function points = wb_lidar_get_layer_point_cloud(tag,index) +% Usage: wb_lidar_get_layer_point_cloud(tag,index) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_lidar_get_horizontal_resolution', tag); +for n = 1:size + obj = calllib('libController', 'wb_lidar_get_point', tag, n - 1 + index * size); + points(n) = obj.Value; +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_range_image.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_range_image.m new file mode 100644 index 000000000..6d9935d12 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_layer_range_image.m @@ -0,0 +1,9 @@ +function range = wb_lidar_get_layer_range_image(tag,layer) +% Usage: wb_lidar_get_layer_range_image(tag,layer) +% Matlab API for Webots +% Online documentation is available here + +width = calllib('libController', 'wb_lidar_get_horizontal_resolution', tag); +obj = calllib('libController', 'wb_lidar_get_layer_range_image', tag, layer); +setdatatype(obj, 'singlePtr', width, 1); +range = get(obj, 'Value')'; % fits the input format of the matlab imagesc() function diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_point_cloud.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_point_cloud.m new file mode 100644 index 000000000..9613892b6 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_point_cloud.m @@ -0,0 +1,21 @@ +function points = wb_lidar_get_point_cloud(tag) +% Usage: wb_lidar_get_point_cloud(tag) +% Matlab API for Webots +% Online documentation is available here + +rate = calllib('libController', 'wb_lidar_get_sampling_period', tag); +point_cloud_enable = calllib('libController', 'wb_lidar_is_point_cloud_enabled', tag); +if (rate <= 0) || (point_cloud_enable == false) + points = [] + if (rate <= 0) + calllib('libController', 'wb_console_print', 'Warning: wb_lidar_get_point_cloud() called for a disabled lidar! Please call wb_lidar_enable() before.', 2); + else + calllib('libController', 'wb_console_print', 'Warning: wb_lidar_get_point_cloud() called for a lidar with point cloud disabled! Please call wb_lidar_enable_point_cloud() before.', 2); + end +else + size = calllib('libController', 'wb_lidar_get_number_of_points', tag); + for n = 1:size + obj = calllib('libController', 'wb_lidar_get_point', tag, n - 1); + points(n) = obj.Value; + end +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_range_image.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_range_image.m new file mode 100644 index 000000000..e3d60f8d0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_lidar_get_range_image.m @@ -0,0 +1,10 @@ +function range = wb_lidar_get_range_image(tag) +% Usage: wb_lidar_get_range_image(tag) +% Matlab API for Webots +% Online documentation is available here + +width = calllib('libController', 'wb_lidar_get_horizontal_resolution', tag); +height = calllib('libController', 'wb_lidar_get_number_of_layers', tag); +obj = calllib('libController', 'wb_lidar_get_range_image', tag); +setdatatype(obj, 'singlePtr', width, height); +range = get(obj, 'Value')'; % fits the input format of the matlab imagesc() function diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_light_sensor_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_light_sensor_get_lookup_table.m new file mode 100644 index 000000000..abc1845a1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_light_sensor_get_lookup_table.m @@ -0,0 +1,9 @@ +function wb_light_sensor_get_lookup_table(tag) +% Usage: wb_light_sensor_get_lookup_table(tag) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_light_sensor_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_light_sensor_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_mouse_get_state.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_mouse_get_state.m new file mode 100644 index 000000000..950ddcb50 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_mouse_get_state.m @@ -0,0 +1,7 @@ +function result = wb_mouse_get_state() +% Usage: wb_mouse_get_state() +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_mouse_get_state_pointer'); +result = obj.Value; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_pen_set_ink_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_pen_set_ink_color.m new file mode 100644 index 000000000..a7d197f80 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_pen_set_ink_color.m @@ -0,0 +1,8 @@ +function wb_pen_set_ink_color(tag, color, density) +% Usage: wb_pen_set_ink_color(tag, color, density) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(color) == 3, 'Invalid ''color'' argument: [red green blue] array expected'); +colorbytes = 65536 * 255 * color(1) + 256 * 255 * color(2) + 255 * color(3); +calllib('libController', 'wb_pen_set_ink_color', tag, colorbytes, density); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_radar_get_targets.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_radar_get_targets.m new file mode 100644 index 000000000..da93d273a --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_radar_get_targets.m @@ -0,0 +1,16 @@ +function targets = wb_radar_get_targets(tag) +% Usage: wb_radar_get_targets(tag) +% Matlab API for Webots +% Online documentation is available here + +rate = calllib('libController', 'wb_radar_get_sampling_period', tag); +if rate <= 0 + targets = [] + calllib('libController', 'wb_console_print', 'Warning: wb_radar_get_targets() called for a disabled radar! Please call wb_radar_enable() before.', 2); +else + size = calllib('libController', 'wb_radar_get_number_of_targets', tag); + for n = 1:size + obj = calllib('libController', 'wb_radar_get_target', tag, n - 1); + targets(n) = obj.Value; + end +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_get_range_image.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_get_range_image.m new file mode 100644 index 000000000..652101da0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_get_range_image.m @@ -0,0 +1,10 @@ +function range = wb_range_finder_get_range_image(tag) +% Usage: wb_range_finder_get_range_image(tag) +% Matlab API for Webots +% Online documentation is available here + +width = calllib('libController', 'wb_range_finder_get_width', tag); +height = calllib('libController', 'wb_range_finder_get_height', tag); +obj = calllib('libController', 'wb_range_finder_get_range_image', tag); +setdatatype(obj, 'singlePtr', width, height); +range = get(obj, 'Value')'; % fits the input format of the matlab imagesc() function diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_image_get_depth.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_image_get_depth.m new file mode 100644 index 000000000..17707be9d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_range_finder_image_get_depth.m @@ -0,0 +1,6 @@ +function depth = wb_range_finder_image_get_depth(image,width,x,y) +% Usage: wb_range_finder_image_get_depth(image,width,x,y) +% Matlab API for Webots +% Online documentation is available here + +depth = image(x,y); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_data.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_data.m new file mode 100644 index 000000000..844b663d6 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_data.m @@ -0,0 +1,22 @@ +function result = wb_receiver_get_data(tag,type) +% Usage: wb_receiver_get_data(tag) +% Matlab API for Webots +% Online documentation is available here + +pointer = calllib('libController', 'wb_receiver_get_data', tag); +size = calllib('libController', 'wb_receiver_get_data_size', tag); +if nargin < 2 + result = pointer; +elseif strcmp(type,'uint8') + setdatatype(pointer, 'uint8Ptr', size); + result = get(pointer, 'Value'); +elseif strcmp(type,'double') + size = size/8; % sizeof(double) + setdatatype(pointer, 'doublePtr', size); + result = get(pointer, 'Value'); +elseif strcmp(type,'string') + setdatatype(pointer, 'uint8Ptr', 1, size); + result = char(get(pointer, 'Value')); +else + result = ''; +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_emitter_direction.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_emitter_direction.m new file mode 100644 index 000000000..5db9607ea --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_receiver_get_emitter_direction.m @@ -0,0 +1,8 @@ +function result = wb_receiver_get_emitter_direction(tag) +% Usage: wb_receiver_get_emitter_direction(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_receiver_get_emitter_direction', tag); +setdatatype(obj, 'doublePtr', 1, 3); +result = get(obj, 'Value')'; diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_cleanup.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_cleanup.m new file mode 100644 index 000000000..6105f75e4 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_cleanup.m @@ -0,0 +1,4 @@ +function wb_robot_cleanup() +% Usage: wb_robot_cleanup() +% Matlab API for Webots +% This is a dummy function: it does nothing diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_init.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_init.m new file mode 100644 index 000000000..0c9694da0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_robot_init.m @@ -0,0 +1,4 @@ +function wb_robot_init() +% Usage: wb_robot_init() +% Matlab API for Webots +% This is a dummy function: it does nothing diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_orientation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_orientation.m new file mode 100644 index 000000000..23f75e8db --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_orientation.m @@ -0,0 +1,7 @@ +function result = wb_skin_get_bone_orientation(tag, index, absolute) +% Usage: wb_skin_get_bone_orientation(tag, index, absolute) +% Matlab API for Webots + +obj = calllib('libController', 'wb_skin_get_bone_orientation', tag, index, absolute); +setdatatype(obj,'doublePtr', 1, 4); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_position.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_position.m new file mode 100644 index 000000000..e71bd5155 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_get_bone_position.m @@ -0,0 +1,7 @@ +function result = wb_skin_get_bone_position(tag, index, absolute) +% Usage: wb_skin_get_bone_position(tag, index, absolute) +% Matlab API for Webots + +obj = calllib('libController', 'wb_skin_get_bone_position', tag, index, absolute); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_orientation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_orientation.m new file mode 100644 index 000000000..968b5b82b --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_orientation.m @@ -0,0 +1,6 @@ +function wb_skin_set_bone_orientation(tag, index, values, absolute) +% Usage: wb_skin_set_bone_orientation(tag, index, values, absolute) +% Matlab API for Webots + +assert(numel(values) == 4, 'Invalid ''values'' argument: 1x4 or 4x1 array expected'); +calllib('libController', 'wb_skin_set_bone_orientation', tag, index, values, absolute); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_position.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_position.m new file mode 100644 index 000000000..a9e70681a --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_skin_set_bone_position.m @@ -0,0 +1,6 @@ +function wb_skin_set_bone_position(tag, index, values, absolute) +% Usage: wb_skin_set_bone_position(tag, index, values, absolute) +% Matlab API for Webots + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_skin_set_bone_position', tag, index, values, absolute); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_color.m new file mode 100644 index 000000000..8458fdf37 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_color.m @@ -0,0 +1,8 @@ +function color = wb_supervisor_field_get_mf_color(field, index) +% Usage: wb_supervisor_field_get_mf_color(field, index) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_mf_color', field, index); +setdatatype(obj,'doublePtr', 1, 3); +color = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_rotation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_rotation.m new file mode 100644 index 000000000..9f10670a0 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_rotation.m @@ -0,0 +1,8 @@ +function rotation = wb_supervisor_field_get_mf_rotation(field, index) +% Usage: wb_supervisor_field_get_mf_rotation(field, index) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_mf_rotation', field, index); +setdatatype(obj,'doublePtr', 1, 4); +rotation = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec2f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec2f.m new file mode 100644 index 000000000..09703b8a3 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec2f.m @@ -0,0 +1,8 @@ +function vec2f = wb_supervisor_field_get_mf_vec2f(field, index) +% Usage: wb_supervisor_field_get_mf_vec2f(field, index) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_mf_vec2f', field, index); +setdatatype(obj,'doublePtr', 1, 2); +vec2f = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec3f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec3f.m new file mode 100644 index 000000000..f558732ed --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_mf_vec3f.m @@ -0,0 +1,8 @@ +function vec3f = wb_supervisor_field_get_mf_vec3f(field, index) +% Usage: wb_supervisor_field_get_mf_vec3f(field, index) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_mf_vec3f', field, index); +setdatatype(obj,'doublePtr', 1, 3); +vec3f = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_color.m new file mode 100644 index 000000000..6938509f7 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_color.m @@ -0,0 +1,8 @@ +function color = wb_supervisor_field_get_sf_color(field) +% Usage: wb_supervisor_field_get_sf_color(field) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_sf_color', field); +setdatatype(obj,'doublePtr', 1, 3); +color = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_rotation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_rotation.m new file mode 100644 index 000000000..6e8cd9ad5 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_rotation.m @@ -0,0 +1,8 @@ +function rotation = wb_supervisor_field_get_sf_rotation(fieldref) +% Usage: wb_supervisor_field_get_sf_rotation(fieldref) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_sf_rotation', fieldref); +setdatatype(obj,'doublePtr', 1, 4); +rotation = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec2f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec2f.m new file mode 100644 index 000000000..8f181aa0e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec2f.m @@ -0,0 +1,8 @@ +function vec2f = wb_supervisor_field_get_sf_vec2f(fieldref) +% Usage: wb_supervisor_field_get_sf_vec2f(fieldref) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_sf_vec2f', fieldref); +setdatatype(obj,'doublePtr', 1, 2); +vec2f = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec3f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec3f.m new file mode 100644 index 000000000..9f75a4714 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_get_sf_vec3f.m @@ -0,0 +1,8 @@ +function vec3f = wb_supervisor_field_get_sf_vec3f(fieldref) +% Usage: wb_supervisor_field_get_sf_vec3f(fieldref) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_field_get_sf_vec3f', fieldref); +setdatatype(obj,'doublePtr', 1, 3); +vec3f = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_color.m new file mode 100644 index 000000000..7afd05b0d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_color.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_insert_mf_color(fieldref, index, values) +% Usage: wb_supervisor_field_insert_mf_color(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_insert_mf_color', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_rotation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_rotation.m new file mode 100644 index 000000000..c2239b3f1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_rotation.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_insert_mf_rotation(fieldref, index, values) +% Usage: wb_supervisor_field_insert_mf_rotation(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 4, 'Invalid ''values'' argument: 1x4 or 4x1 array expected'); +calllib('libController', 'wb_supervisor_field_insert_mf_rotation', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec2f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec2f.m new file mode 100644 index 000000000..4930ed150 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec2f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_insert_mf_vec2f(fieldref, index, values) +% Usage: wb_supervisor_field_insert_mf_vec2f(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 2, 'Invalid ''values'' argument: 1x2 or 2x1 array expected'); +calllib('libController', 'wb_supervisor_field_insert_mf_vec2f', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec3f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec3f.m new file mode 100644 index 000000000..07e2b4f8a --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_insert_mf_vec3f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_insert_mf_vec3f(fieldref, index, values) +% Usage: wb_supervisor_field_insert_mf_vec3f(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_insert_mf_vec3f', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_color.m new file mode 100644 index 000000000..7bd319170 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_color.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_mf_color(fieldref, index, values) +% Usage: wb_supervisor_field_set_mf_color(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_mf_color', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_rotation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_rotation.m new file mode 100644 index 000000000..cf3f74804 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_rotation.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_mf_rotation(fieldref, index, values) +% Usage: wb_supervisor_field_set_mf_rotation(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 4, 'Invalid ''values'' argument: 1x4 or 4x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_mf_rotation', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec2f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec2f.m new file mode 100644 index 000000000..f26125f21 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec2f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_mf_vec2f(fieldref, index, values) +% Usage: wb_supervisor_field_set_mf_vec2f(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 2, 'Invalid ''values'' argument: 1x2 or 2x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_mf_vec2f', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec3f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec3f.m new file mode 100644 index 000000000..01601a3dc --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_mf_vec3f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_mf_vec3f(fieldref, index, values) +% Usage: wb_supervisor_field_set_mf_vec3f(fieldref, index, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_mf_vec3f', fieldref, index, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_color.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_color.m new file mode 100644 index 000000000..cf3432c28 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_color.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_sf_color(fieldref, values) +% Usage: wb_supervisor_field_set_sf_color(fieldref, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_sf_color', fieldref, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_rotation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_rotation.m new file mode 100644 index 000000000..a1c2feed5 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_rotation.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_sf_rotation(fieldref, values) +% Usage: wb_supervisor_field_set_sf_rotation(fieldref, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 4, 'Invalid ''values'' argument: 1x4 or 4x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_sf_rotation', fieldref, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec2f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec2f.m new file mode 100644 index 000000000..b41390d21 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec2f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_sf_vec2f(fieldref, values) +% Usage: wb_supervisor_field_set_sf_vec2f(fieldref, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 2, 'Invalid ''values'' argument: 1x2 or 2x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_sf_vec2f', fieldref, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec3f.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec3f.m new file mode 100644 index 000000000..54e450940 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_field_set_sf_vec3f.m @@ -0,0 +1,7 @@ +function wb_supervisor_field_set_sf_vec3f(fieldref, values) +% Usage: wb_supervisor_field_set_sf_vec3f(fieldref, values) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(values) == 3, 'Invalid ''values'' argument: 1x3 or 3x1 array expected'); +calllib('libController', 'wb_supervisor_field_set_sf_vec3f', fieldref, values); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_get_movie_status.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_get_movie_status.m new file mode 100644 index 000000000..d4ea60a92 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_get_movie_status.m @@ -0,0 +1,10 @@ +function result = wb_supervisor_get_movie_status() + +% DEPRECATED +warning('wb_supervisor_get_movie_status() is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); + +% Usage: wb_supervisor_get_movie_status() +% Matlab API for Webots +% Online documentation is available here + +result = calllib('libController', 'wb_supervisor_get_movie_status'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_load_world.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_load_world.m new file mode 100644 index 000000000..ce743cefe --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_load_world.m @@ -0,0 +1,6 @@ +function wb_supervisor_load_world(filename) +% Usage: wb_supervisor_load_world(filename) +% Matlab API for Webots +% Online documentation is available here + +calllib('libController', 'wb_supervisor_load_world', filename); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_movie_get_status.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_movie_get_status.m new file mode 100644 index 000000000..0236c82e7 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_movie_get_status.m @@ -0,0 +1,9 @@ +function result = wb_supervisor_movie_get_status() +% DEPRECATED +warning('wb_supervisor_movie_get_status() is deprecated, use wb_supervisor_movie_is_ready() and wb_supervisor_movie_failed() instead'); + +% Usage: wb_supervisor_movie_get_status() +% Matlab API for Webots +% Online documentation is available here + +result = calllib('libController', 'wb_supervisor_movie_get_status'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_center_of_mass.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_center_of_mass.m new file mode 100644 index 000000000..300af6a8e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_center_of_mass.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_node_get_center_of_mass(noderef) +% Usage: wb_supervisor_node_get_center_of_mass(noderef) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_node_get_center_of_mass', noderef); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_point.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_point.m new file mode 100644 index 000000000..b51418a15 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_point.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_node_get_contact_point(noderef, index) +% Usage: wb_supervisor_node_get_contact_point(noderef, index) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_node_get_contact_point', noderef, index); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_points.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_points.m new file mode 100644 index 000000000..11a67f8f6 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_contact_points.m @@ -0,0 +1,19 @@ +function contact_points = wb_supervisor_node_get_contact_points(noderef,include_descendants) +% Usage: wb_supervisor_node_get_contact_points(noderef,include_descendants) +% Matlab API for Webots +% Online documentation is available here + +% need to stay in memory until wb_robot_step() +persistent pointer; + +size = libpointer('int32Ptr', 0); +contact_points_ref = calllib('libController','wb_supervisor_node_get_contact_points',noderef,include_descendants,size); +setdatatype(contact_points_ref, 'WbContactPointPtr'); +if size.value <= 0 + contact_points = []; +else + for i = 1:size.value + contact_points_ref_array = contact_points_ref + (i - 1); + contact_points(i) = contact_points_ref_array.value; + end +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_orientation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_orientation.m new file mode 100644 index 000000000..ab5ab481d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_orientation.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_node_get_orientation(noderef) +% Usage: wb_supervisor_node_get_orientation(noderef) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_node_get_orientation', noderef); +setdatatype(obj,'doublePtr', 3, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_pose.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_pose.m new file mode 100644 index 000000000..5f78af9cd --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_pose.m @@ -0,0 +1,13 @@ +function result = wb_supervisor_node_get_pose(noderef, noderef_from) +% Usage: wb_supervisor_node_get_pose(noderef, noderef_from) +% Matlab API for Webots +% Online documentation is available here + +if nargin < 2 + nullpointer = libpointer; + obj = calllib('libController', 'wb_supervisor_node_get_pose', noderef, nullpointer); +else + obj = calllib('libController', 'wb_supervisor_node_get_pose', noderef, noderef_from); +end +setdatatype(obj,'doublePtr', 4, 4); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_position.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_position.m new file mode 100644 index 000000000..08455c08f --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_position.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_node_get_position(noderef) +% Usage: wb_supervisor_node_get_position(noderef) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_node_get_position', noderef); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_velocity.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_velocity.m new file mode 100644 index 000000000..9f00ad6d9 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_node_get_velocity.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_node_get_velocity(noderef) +% Usage: wb_supervisor_node_get_velocity(noderef) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_node_get_velocity', noderef); +setdatatype(obj,'doublePtr', 1, 6); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_save_world.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_save_world.m new file mode 100644 index 000000000..687aa7740 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_save_world.m @@ -0,0 +1,11 @@ +function result = wb_supervisor_save_world(filename) +% Usage: wb_supervisor_save_world(filename) +% Matlab API for Webots +% Online documentation is available here + +if nargin == 0 + nullpointer = libpointer; + result = calllib('libController', 'wb_supervisor_save_world', nullpointer); +else + result = calllib('libController', 'wb_supervisor_save_world', filename); +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_set_label.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_set_label.m new file mode 100644 index 000000000..60218cbb8 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_set_label.m @@ -0,0 +1,8 @@ +function wb_supervisor_set_label(id, string, xpos, ypos, size, color, transparency, font) +% Usage: wb_supervisor_set_label(id, string, xpos, ypos, size, color, transparency, font) +% Matlab API for Webots +% Online documentation is available here + +assert(numel(color) == 3, 'Invalid ''color'' argument: [red green blue] array expected'); +colorbytes = 65536 * 255 * color(1) + 256 * 255 * color(2) + 255 * color(3); +obj = calllib('libController', 'wb_supervisor_set_label', id, string, xpos, ypos, size, colorbytes, transparency, font); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_physics_reset.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_physics_reset.m new file mode 100644 index 000000000..1e2424b87 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_physics_reset.m @@ -0,0 +1,9 @@ +function wb_supervisor_simulation_physics_reset() +% DEPRECATED use wb_supervisor_simulation_reset_physics() instead +warning('wb_supervisor_simulation_physics_reset() is deprecated, use wb_supervisor_simulation_reset_physics() instead'); + +% Usage: wb_supervisor_simulation_physics_reset() +% Matlab API for Webots +% Online documentation is available here + +calllib('libController', 'wb_supervisor_simulation_reset_physics'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_revert.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_revert.m new file mode 100644 index 000000000..a74df0ae8 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_simulation_revert.m @@ -0,0 +1,6 @@ +function wb_supervisor_simulation_revert() +% Usage: wb_supervisor_simulation_revert() +% Matlab API for Webots +% Online documentation is available here + +calllib('libController', 'wb_supervisor_simulation_revert'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_start_movie.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_start_movie.m new file mode 100644 index 000000000..ff94a0e9b --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_start_movie.m @@ -0,0 +1,10 @@ +function wb_supervisor_start_movie(filename, width, height, codec, quality, acceleration, caption) + +% DEPRECATED use wb_supervisor_movie_start_recording() instead +warning('wb_supervisor_start_movie() is deprecated, use wb_supervisor_movie_start_recording() instead'); + +% Usage: wb_supervisor_start_movie(filename, width, height, codec, quality, acceleration, caption) +% Matlab API for Webots +% Online documentation is available here + +calllib('libController', 'wb_supervisor_start_movie', filename, width, height, codec, quality, acceleration, caption); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_stop_movie.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_stop_movie.m new file mode 100644 index 000000000..b77137a5e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_stop_movie.m @@ -0,0 +1,10 @@ +function wb_supervisor_stop_movie() + +% DEPRECATED use wb_supervisor_movie_stop_recording() instead +warning('wb_supervisor_stop_movie() is deprecated, use wb_supervisor_movie_stop_recording() instead'); + +% Usage: wb_supervisor_stop_movie() +% Matlab API for Webots +% Online documentation is available here + +calllib('libController', 'wb_supervisor_stop_movie'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_orientation.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_orientation.m new file mode 100644 index 000000000..875bd2a9e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_orientation.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_virtual_reality_headset_get_orientation() +% Usage: wb_supervisor_virtual_reality_headset_get_orientation() +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_virtual_reality_headset_get_orientation'); +setdatatype(obj,'doublePtr', 1, 9); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_position.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_position.m new file mode 100644 index 000000000..d0fd62c47 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_virtual_reality_headset_get_position.m @@ -0,0 +1,8 @@ +function result = wb_supervisor_virtual_reality_headset_get_position() +% Usage: wb_supervisor_virtual_reality_headset_get_position() +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_supervisor_virtual_reality_headset_get_position'); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_world_save.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_world_save.m new file mode 100644 index 000000000..0b5d32523 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_supervisor_world_save.m @@ -0,0 +1,11 @@ +function result = wb_supervisor_world_save(filename) +% Usage: wb_supervisor_world_save(filename) +% Matlab API for Webots +% Online documentation is available here + +if nargin == 0 + nullpointer = libpointer; + result = calllib('libController', 'wb_supervisor_world_save', nullpointer); +else + result = calllib('libController', 'wb_supervisor_world_save', filename); +end diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_lookup_table.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_lookup_table.m new file mode 100644 index 000000000..c180e6bef --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_lookup_table.m @@ -0,0 +1,9 @@ +function wb_touch_sensor_get_lookup_table(tag, sampling_period) +% Usage: wb_touch_sensor_get_lookup_table(tag, sampling_period) +% Matlab API for Webots +% Online documentation is available here + +size = calllib('libController', 'wb_touch_sensor_get_lookup_table_size', tag); +pointer = calllib('libController', 'wb_touch_sensor_get_lookup_table', tag); +setdatatype(pointer, 'doublePtr', 3 * size, 1); +result = get(pointer, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_values.m b/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_values.m new file mode 100644 index 000000000..1e9e49a1d --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/matlab/wb_touch_sensor_get_values.m @@ -0,0 +1,8 @@ +function result = wb_touch_sensor_get_values(tag) +% Usage: wb_touch_sensor_get_values(tag) +% Matlab API for Webots +% Online documentation is available here + +obj = calllib('libController', 'wb_touch_sensor_get_values', tag); +setdatatype(obj,'doublePtr', 1, 3); +result = get(obj, 'Value'); diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py b/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py new file mode 100644 index 000000000..aa71de125 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/__init__.py @@ -0,0 +1,47 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.field import Field # noqa +from controller.node import Node # noqa +from controller.ansi_codes import AnsiCodes # noqa +from controller.accelerometer import Accelerometer # noqa +from controller.altimeter import Altimeter # noqa +from controller.brake import Brake # noqa +from controller.camera import Camera # noqa +from controller.compass import Compass # noqa +from controller.connector import Connector # noqa +from controller.display import Display # noqa +from controller.distance_sensor import DistanceSensor # noqa +from controller.emitter import Emitter # noqa +from controller.gps import GPS # noqa +from controller.gyro import Gyro # noqa +from controller.inertial_unit import InertialUnit # noqa +from controller.led import LED # noqa +from controller.lidar import Lidar # noqa +from controller.light_sensor import LightSensor # noqa +from controller.motor import Motor # noqa +from controller.position_sensor import PositionSensor # noqa +from controller.radar import Radar # noqa +from controller.range_finder import RangeFinder # noqa +from controller.receiver import Receiver # noqa +from controller.robot import Robot # noqa +from controller.skin import Skin # noqa +from controller.speaker import Speaker # noqa +from controller.supervisor import Supervisor # noqa +from controller.touch_sensor import TouchSensor # noqa +from controller.keyboard import Keyboard # noqa +from controller.mouse import Mouse # noqa +from controller.mouse import MouseState # noqa +from controller.joystick import Joystick # noqa +from controller.motion import Motion # noqa diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/accelerometer.py b/webots_ros2_driver/webots/lib/controller/python/controller/accelerometer.py new file mode 100644 index 000000000..1850a69ee --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/accelerometer.py @@ -0,0 +1,43 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class Accelerometer(Sensor): + wb.wb_accelerometer_get_values.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_accelerometer_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_accelerometer_enable + self._get_sampling_period = wb.wb_accelerometer_get_sampling_period + super().__init__(name, sampling_period) + + def getValues(self) -> typing.List[float]: + return self.value + + def getLookupTable(self) -> typing.List[float]: + return self.lookup_table + + @property + def lookup_table(self) -> typing.List[float]: + size = wb.wb_accelerometer_get_lookup_table_size(self._tag) + return wb.wb_accelerometer_get_lookup_table(self._tag)[: 3 * size] + + @property + def value(self) -> typing.List[float]: + return wb.wb_accelerometer_get_values(self._tag)[:3] diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/altimeter.py b/webots_ros2_driver/webots/lib/controller/python/controller/altimeter.py new file mode 100644 index 000000000..cdea3b8ec --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/altimeter.py @@ -0,0 +1,34 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class Altimeter(Sensor): + wb.wb_altimeter_get_value.restype = ctypes.c_double + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_altimeter_enable + self._get_sampling_period = wb.wb_altimeter_get_sampling_period + super().__init__(name, sampling_period) + + def getValue(self) -> float: + return self.value + + @property + def value(self) -> float: + return wb.wb_altimeter_get_value(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/ansi_codes.py b/webots_ros2_driver/webots/lib/controller/python/controller/ansi_codes.py new file mode 100644 index 000000000..e17f0ef03 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/ansi_codes.py @@ -0,0 +1,38 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.constants import constant + + +class AnsiCodes: + RESET = constant('ANSI_RESET', type=str) + BOLD = constant('ANSI_BOLD', type=str) + UNDERLINE = constant('ANSI_UNDERLINE', type=str) + BLACK_BACKGROUND = constant('ANSI_BLACK_BACKGROUND', type=str) + RED_BACKGROUND = constant('ANSI_RED_BACKGROUND', type=str) + GREEN_BACKGROUND = constant('ANSI_GREEN_BACKGROUND', type=str) + YELLOW_BACKGROUND = constant('ANSI_YELLOW_BACKGROUND', type=str) + BLUE_BACKGROUND = constant('ANSI_BLUE_BACKGROUND', type=str) + MAGENTA_BACKGROUND = constant('ANSI_MAGENTA_BACKGROUND', type=str) + CYAN_BACKGROUND = constant('ANSI_CYAN_BACKGROUND', type=str) + WHITE_BACKGROUND = constant('ANSI_WHITE_BACKGROUND', type=str) + BLACK_FOREGROUND = constant('ANSI_BLACK_FOREGROUND', type=str) + RED_FOREGROUND = constant('ANSI_RED_FOREGROUND', type=str) + GREEN_FOREGROUND = constant('ANSI_GREEN_FOREGROUND', type=str) + YELLOW_FOREGROUND = constant('ANSI_YELLOW_FOREGROUND', type=str) + BLUE_FOREGROUND = constant('ANSI_BLUE_FOREGROUND', type=str) + MAGENTA_FOREGROUND = constant('ANSI_MAGENTA_FOREGROUND', type=str) + CYAN_FOREGROUND = constant('ANSI_CYAN_FOREGROUND', type=str) + WHITE_FOREGROUND = constant('ANSI_WHITE_FOREGROUND', type=str) + CLEAR_SCREEN = constant('ANSI_CLEAR_SCREEN', type=str) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/brake.py b/webots_ros2_driver/webots/lib/controller/python/controller/brake.py new file mode 100644 index 000000000..1f9f4d81b --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/brake.py @@ -0,0 +1,57 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.constants import constant +from controller.device import Device +from controller.wb import wb +from typing import Union + + +class Brake(Device): + ROTATIONAL = constant('ROTATIONAL') + LINEAR = constant('LINEAR') + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def setDampingConstant(self, c: float): + wb.wb_brake_set_damping_constant(self._tag, ctypes.c_double(c)) + + dampingConstant = property(fset=setDampingConstant) + + def getType(self) -> int: + return self.type + + def getMotor(self): + return self.motor + + def getPositionSensor(self): + return self.position_sensor + + @property + def motor(self): + from controller.motor import Motor + tag = wb.wb_brake_get_motor(self._tag) + return None if tag == 0 else Motor(tag) + + @property + def position_sensor(self): + from controller.position_sensor import PositionSensor + tag = wb.wb_brake_get_position_sensor(self._tag) + return None if tag == 0 else PositionSensor(tag) + + @property + def type(self) -> int: + return wb.wb_brake_get_type(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/camera.py b/webots_ros2_driver/webots/lib/controller/python/controller/camera.py new file mode 100644 index 000000000..b903af73e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/camera.py @@ -0,0 +1,269 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.sensor import Sensor +from controller.wb import wb +from typing import List, Union + + +class Camera(Sensor): + wb.wb_camera_get_image.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_camera_recognition_get_segmentation_image.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_camera_get_fov.restype = ctypes.c_double + wb.wb_camera_get_exposure.restype = ctypes.c_double + wb.wb_camera_get_focal_distance.restype = ctypes.c_double + wb.wb_camera_get_focal_length.restype = ctypes.c_double + wb.wb_camera_get_max_fov.restype = ctypes.c_double + wb.wb_camera_get_min_fov.restype = ctypes.c_double + wb.wb_camera_get_max_focal_distance.restype = ctypes.c_double + wb.wb_camera_get_min_focal_distance.restype = ctypes.c_double + wb.wb_camera_get_near.restype = ctypes.c_double + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_camera_enable + self._get_sampling_period = wb.wb_camera_get_sampling_period + super().__init__(name, sampling_period) + + def getExposure(self) -> float: + return self.exposure + + def getFocalDistance(self) -> float: + return self.focal_distance + + def getFocalLength(self) -> float: + return self.focal_length + + def getFov(self) -> float: + return self.fov + + def getHeight(self) -> int: + return self.height + + def getImage(self) -> bytes: + return self.image + + def getImageArray(self) -> List[List[List[int]]]: + array = [] + image = self.image + if not image: + return None + i = 0 + for x in range(self.width): + line = [] + for y in range(self.height): + line.append([image[i + 2], image[i + 1], image[i]]) # RGB pixel + i += 4 + array.append(line) + return array + + @staticmethod + def imageGetRed(image: bytes, width: int, x: int, y: int) -> int: + return image[4 * (y * width + x) + 2] + + @staticmethod + def imageGetGreen(image: bytes, width: int, x: int, y: int) -> int: + return image[4 * (y * width + x) + 1] + + @staticmethod + def imageGetBlue(image: bytes, width: int, x: int, y: int) -> int: + return image[4 * (y * width + x)] + + @staticmethod + def imageGetGray(image: bytes, width: int, x: int, y: int) -> int: + return (image[4 * (y * width + x) + 2] + image[4 * (y * width + x) + 1] + image[4 * (y * width + x)]) / 3 + + def getMaxFocalDistance(self) -> float: + return self.max_focal_distance + + def getMaxFov(self) -> float: + return self.max_fov + + def getMinFocalDistance(self) -> float: + return self.min_focal_distance + + def getMinFov(self) -> float: + return self.min_fov + + def getNear(self) -> float: + return self.near + + def getWidth(self) -> int: + return self.width + + def saveImage(self, filename: str, quality: int) -> int: + return wb.wb_camera_save_image(self._tag, str.encode(filename), quality) + + def setExposure(self, exposure: float): + self.exposure = exposure + + def setFocalDistance(self, d: float): + self.focal_distance = d + + def setFov(self, f: float): + self.fov = f + + @property + def image(self): + return wb.wb_camera_get_image(self._tag) + + @property + def exposure(self) -> float: + return wb.wb_camera_get_exposure(self._tag) + + @exposure.setter + def exposure(self, exposure: float): + wb.wb_camera_set_exposure(self._tag, ctypes.c_double(exposure)) + + @property + def focal_distance(self) -> float: + return wb.wb_camera_get_focal_distance(self._tag) + + @focal_distance.setter + def focal_distance(self, focal_distance: float): + wb.wb_camera_set_focal_distance(self._tag, ctypes.c_double(focal_distance)) + + @property + def focal_length(self) -> float: + return wb.wb_camera_get_focal_length(self._tag) + + @property + def max_focal_distance(self) -> float: + return wb.wb_camera_get_max_focal_distance(self._tag) + + @property + def min_focal_distance(self) -> float: + return wb.wb_camera_get_min_focal_distance(self._tag) + + @property + def fov(self) -> float: + return wb.wb_camera_get_fov(self._tag) + + @fov.setter + def fov(self, fov: float): + wb.wb_camera_set_fov(self._tag, ctypes.c_double(fov)) + + @property + def height(self) -> int: + return wb.wb_camera_get_height(self._tag) + + @property + def min_fov(self) -> float: + return wb.wb_camera_get_min_fov(self._tag) + + @property + def max_fov(self) -> float: + return wb.wb_camera_get_max_fov(self._tag) + + @property + def near(self) -> float: + return wb.wb_camera_get_near(self._tag) + + @property + def width(self) -> int: + return wb.wb_camera_get_width(self._tag) + + class CameraRecognitionObject(ctypes.Structure): + _fields_ = [('id', ctypes.c_int), + ('position', ctypes.c_double * 3), + ('orientation', ctypes.c_double * 4), + ('size', ctypes.c_double * 2), + ('position_on_image', ctypes.c_int * 2), + ('size_on_image', ctypes.c_int * 2), + ('number_of_colors', ctypes.c_int), + ('colors', ctypes.POINTER(ctypes.c_double)), + ('_model', ctypes.c_char_p)] + + def getId(self) -> int: + return self.id + + def getPosition(self) -> List[float]: + return self.position + + def getOrientation(self) -> List[float]: + return self.orientation + + def getSize(self) -> List[float]: + return self.size + + def getPositionOnImage(self) -> List[int]: + return self.position_on_image + + def getSizeOnImage(self) -> List[int]: + return self.size_on_image + + def getNumberOfColors(self) -> int: + return self.number_of_colors + + def getColors(self) -> List[float]: + return self.colors + + def getModel(self) -> str: + return self.model + + @property + def model(self) -> str: + return self._model.decode() + + wb.wb_camera_recognition_get_objects.restype = ctypes.POINTER(CameraRecognitionObject) + + def getRecognitionNumberOfObjects(self) -> int: + return wb.wb_camera_recognition_get_number_of_objects(self._tag) + + def getRecognitionObjects(self) -> List[CameraRecognitionObject]: + return wb.wb_camera_recognition_get_objects(self._tag)[:wb.wb_camera_recognition_get_number_of_objects(self._tag)] + + def getRecognitionSamplingPeriod(self) -> int: + return wb.wb_camera_recognition_get_sampling_period(self._tag) + + def hasRecognition(self) -> bool: + return wb.wb_camera_has_recognition(self._tag) != 0 + + def recognitionDisable(self): + wb.wb_camera_recognition_disable(self._tag) + + def recognitionEnable(self, sampling_period: int): + wb.wb_camera_recognition_enable(self._tag, sampling_period) + + def disableRecognitionSegmentation(self): + wb.wb_camera_recognition_disable_segmentation(self._tag) + + def enableRecognitionSegmentation(self): + wb.wb_camera_recognition_enable_segmentation(self._tag) + + def hasRecognitionSegmentation(self) -> bool: + return wb.wb_camera_recognition_has_segmentation(self._tag) != 0 + + def isRecognitionSegmentationEnabled(self) -> bool: + return wb.wb_camera_recognition_is_segmentation_enabled(self._tag) != 0 + + def getRecognitionSegmentationImage(self) -> bytes: + return wb.wb_camera_recognition_get_segmentation_image(self._tag) + + def getRecognitionSegmentationImageArray(self) -> List[List[List[int]]]: + array = [] + image = self.getRecognitionSegmentationImage() + if not image: + return None + i = 0 + for x in range(self.width): + line = [] + for y in range(self.height): + line.append([image[i + 2], image[i + 1], image[i]]) # RGB pixel + i += 4 + array.append(line) + return array + + def saveRecognitionSegmentationImage(self, filename: str, quality: int) -> int: + return wb.wb_camera_recognition_save_segmentation_image(self._tag, str.encode(filename), quality) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/compass.py b/webots_ros2_driver/webots/lib/controller/python/controller/compass.py new file mode 100644 index 000000000..be4527496 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/compass.py @@ -0,0 +1,43 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class Compass(Sensor): + wb.wb_compass_get_values.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_compass_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_compass_enable + self._get_sampling_period = wb.wb_compass_get_sampling_period + super().__init__(name, sampling_period) + + def getValues(self) -> typing.List[float]: + return self.value + + def getLookupTable(self) -> typing.List[float]: + return self.lookup_table + + @property + def value(self) -> typing.List[float]: + return wb.wb_compass_get_values(self._tag)[:3] + + @property + def lookup_table(self) -> typing.List[float]: + size = wb.wb_compass_get_lookup_table_size(self._tag) + return wb.wb_compass_get_lookup_table(self._tag)[: 3 * size] diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/connector.py b/webots_ros2_driver/webots/lib/controller/python/controller/connector.py new file mode 100644 index 000000000..4e0be5b50 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/connector.py @@ -0,0 +1,55 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.device import Device +from typing import Union + + +class Connector(Device): + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def disablePresence(self): + wb.wb_connector_disable_presence(self._tag) + + def enablePresence(self, sampling_period): + wb.wb_connector_enable_presence(self._tag, sampling_period) + + def getPresence(self) -> int: + return self.presence + + def getPresenceSamplingPeriod(self) -> int: + return self.presence_sampling_period + + def isLocked(self) -> bool: + return self.locked + + def lock(self): + wb.wb_connector_lock(self._tag) + + def unlock(self): + wb.wb_connector_unlock(self._tag) + + @property + def locked(self) -> bool: + return wb.wb_connector_is_locked(self._tag) != 0 + + @property + def presence(self) -> int: + return wb.wb_connector_get_presence(self._tag) + + @property + def presence_sampling_period(self) -> int: + return wb.wb_connector_get_presence_sampling_period(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/constants.py b/webots_ros2_driver/webots/lib/controller/python/controller/constants.py new file mode 100644 index 000000000..362ffee03 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/constants.py @@ -0,0 +1,23 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb + + +def constant(name, type=int): + if type == int: + return ctypes.c_int.in_dll(wb, 'wb_' + name).value + if type == str: + return ctypes.c_char_p.in_dll(wb, 'wb_' + name).value.decode() diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/device.py b/webots_ros2_driver/webots/lib/controller/python/controller/device.py new file mode 100644 index 000000000..290e96fe5 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/device.py @@ -0,0 +1,46 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +import ctypes +from typing import Union + + +class Device: + wb.wb_device_get_name.restype = ctypes.c_char_p + wb.wb_device_get_model.restype = ctypes.c_char_p + + def __init__(self, name: Union[str, int]): + self._tag = name if isinstance(name, int) else wb.wb_robot_get_device(str.encode(name)) + + def getName(self) -> str: + return self.name + + def getModel(self) -> str: + return self.model + + def getNodeType(self) -> int: + return self.node_type + + @property + def name(self) -> str: + return wb.wb_device_get_name(self._tag).decode() + + @property + def model(self) -> str: + return wb.wb_device_get_model(self._tag).decode() + + @property + def node_type(self) -> int: + return wb.wb_device_get_node_type(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/display.py b/webots_ros2_driver/webots/lib/controller/python/controller/display.py new file mode 100644 index 000000000..997f141a1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/display.py @@ -0,0 +1,127 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb +from controller.camera import Camera +from controller.device import Device +from typing import Union, List + + +class Display(Device): + RGB = 3 + RGBA = 4 + ARGB = 5 + BGRA = 6 + ABGR = 7 + wb.wb_display_image_copy.restype = ctypes.c_void_p + wb.wb_display_image_load.restype = ctypes.c_void_p + wb.wb_display_image_new.restype = ctypes.c_void_p + wb.wb_display_image_new.argtypes = [ctypes.c_void_p, + ctypes.c_int, + ctypes.c_int, + ctypes.c_void_p, + ctypes.c_int] + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def attachCamera(self, camera: Camera): + wb.wb_display_attach_camera(self._tag, camera._tag) + + def detachCamera(self): + wb.wb_display_detach_camera(self._tag) + + def drawLine(self, x1: int, y1: int, x2: int, y2: int): + wb.wb_display_draw_line(self._tag, int(x1), int(y1), int(x2), int(y2)) + + def drawOval(self, cx: int, cy: int, a: int, b: int): + wb.wb_display_draw_oval(self._tag, int(cx), int(cy), int(a), int(b)) + + def drawPixel(self, x: int, y: int): + wb.wb_display_draw_pixel(self._tag, int(x), int(y)) + + def drawPolygon(self, x: List[int], y: List[int]): + wb.wb_display_draw_polygon(self._tag, + (ctypes.c_int * len(x))(*x), + (ctypes.c_int * len(y))(*y), + min(len(x), len(y))) + + def drawRectangle(self, x: int, y: int, width: int, height: int): + wb.wb_display_draw_rectangle(self._tag, int(x), int(y), int(width), int(height)) + + def drawText(self, text: str, x: int, y: int): + wb.wb_display_draw_text(self._tag, str.encode(text), int(x), int(y)) + + def fillOval(self, cx: int, cy: int, a: int, b: int): + wb.wb_display_fill_oval(self._tag, int(cx), int(cy), int(a), int(b)) + + def fillPolygon(self, x: List[int], y: List[int]): + wb.wb_display_fill_polygon(self._tag, + (ctypes.c_int * len(x))(*x), + (ctypes.c_int * len(y))(*y), + min(len(x), len(y))) + + def fillRectangle(self, x: int, y: int, width: int, height: int): + wb.wb_display_fill_rectangle(self._tag, int(x), int(y), int(width), int(height)) + + def getHeight(self) -> int: + return self.height + + def getWidth(self) -> int: + return self.width + + def imageCopy(self, x: int, y: int, width: int, height: int) -> int: + return wb.wb_display_image_copy(self._tag, int(x), int(y), int(width), int(height)) + + def imageDelete(self, image): + wb.wb_display_image_delete(self._tag, ctypes.c_void_p(image)) + + def imageNew(self, data: bytes, format: int, width: int, height: int) -> int: + return wb.wb_display_image_new(self._tag, int(width), int(height), data, format) + + def imageLoad(self, filename: str) -> int: + return wb.wb_display_image_load(self._tag, str.encode(filename)) + + def imagePaste(self, image, x: int, y: int, blend: bool): + wb.wb_display_image_paste(self._tag, ctypes.c_void_p(image), int(x), int(y), ctypes.c_int(blend)) + + def imageSave(self, image, filename: str): + wb.wb_display_image_save(self._tag, ctypes.c_void_p(image), str.encode(filename)) + + def setAlpha(self, alpha: float): + wb.wb_display_set_alpha(self._tag, ctypes.c_double(alpha)) + + alpha = property(fset=setAlpha) + + def setColor(self, color: int): + wb.wb_display_set_color(self._tag, color) + + color = property(fset=setColor) + + def setFont(self, font: str, size: int, anti_aliasing: bool): + wb.wb_display_set_font(self._tag, str.encode(font), size, ctypes.c_int(anti_aliasing)) + + def setOpacity(self, opacity: float): + wb.wb_display_set_opacity(self._tag, ctypes.c_double(opacity)) + + opacity = property(fset=setOpacity) + + @property + def width(self) -> int: + return wb.wb_display_get_width(self._tag) + + @property + def height(self) -> int: + return wb.wb_display_get_height(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/distance_sensor.py b/webots_ros2_driver/webots/lib/controller/python/controller/distance_sensor.py new file mode 100644 index 000000000..62fdcf234 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/distance_sensor.py @@ -0,0 +1,77 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb +from controller.sensor import Sensor +from typing import Union, List + + +class DistanceSensor(Sensor): + GENERIC = 0 + INFRA_RED = 1 + SONAR = 2 + LASER = 3 + wb.wb_distance_sensor_get_aperture.restype = ctypes.c_double + wb.wb_distance_sensor_get_max_value.restype = ctypes.c_double + wb.wb_distance_sensor_get_min_value.restype = ctypes.c_double + wb.wb_distance_sensor_get_value.restype = ctypes.c_double + wb.wb_distance_sensor_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_distance_sensor_enable + self._get_sampling_period = wb.wb_distance_sensor_get_sampling_period + super().__init__(name, sampling_period) + + def getAperture(self) -> float: + return self.aperture + + def getLookupTable(self) -> List[float]: + return self.lookup_table[:3 * wb.wb_distance_sensor_get_lookup_table_size(self._tag)] + + def getMaxValue(self) -> float: + return self.max_value + + def getMinValue(self) -> float: + return self.min_value + + def getType(self) -> int: + return self.type + + def getValue(self) -> float: + return self.value + + @property + def aperture(self) -> float: + return wb.wb_distance_sensor_get_aperture(self._tag) + + @property + def lookup_table(self): + return wb.wb_distance_sensor_get_lookup_table(self._tag) + + @property + def max_value(self) -> float: + return wb.wb_distance_sensor_get_max_value(self._tag) + + @property + def min_value(self) -> float: + return wb.wb_distance_sensor_get_min_value(self._tag) + + @property + def type(self) -> int: + return wb.wb_distance_sensor_get_type(self._tag) + + @property + def value(self) -> float: + return wb.wb_distance_sensor_get_value(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/emitter.py b/webots_ros2_driver/webots/lib/controller/python/controller/emitter.py new file mode 100644 index 000000000..50ed4cc78 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/emitter.py @@ -0,0 +1,90 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import struct +import sys +from controller.wb import wb +from controller.device import Device +import ctypes +from typing import Union, List + + +class Emitter(Device): + wb.wb_emitter_get_range.restype = ctypes.c_double + + CHANNEL_BROADCAST = -1 + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def getBufferSize(self) -> int: + return self.buffer_size + + @property + def buffer_size(self) -> int: + return wb.wb_emitter_get_buffer_size(self._tag) + + def send(self, message: Union[str, bytes, List[float]], length: int = None): + if isinstance(message, bytes): + if length is None: + length = len(message) + wb.wb_emitter_send(self._tag, message, length) + elif isinstance(message, str): + wb.wb_emitter_send(self._tag, str.encode(message), len(message)) + elif isinstance(message, list) or isinstance(message, tuple): + length = len(message) + if length == 0: + print('Emitter.send(): empty list', file=sys.stderr) + return + if isinstance(message[0], float): + data_type = 'd' + elif isinstance(message[0], int): + data_type = 'i' + elif isinstance(message[0], bool): + data_type = '?' + else: + print(f'Emitter.send(): unsupported data type list: {type(message)}', file=sys.stderr) + return + pack = struct.pack(f'{length}{data_type}', *message) + wb.wb_emitter_send(self._tag, pack, len(pack)) + else: + print(f'Emitter.send(): unsupported data type: {type(message)}', file=sys.stderr) + + def getChannel(self) -> int: + return self.channel + + @property + def channel(self) -> int: + return wb.wb_emitter_get_channel(self._tag) + + def setChannel(self, channel: int): + self.channel = channel + + @channel.setter + def channel(self, c: int): + wb.wb_emitter_set_channel(self._tag, c) + + def getRange(self) -> float: + return self.range + + @property + def range(self) -> float: + return wb.wb_emitter_get_range(self._tag) + + def setRange(self, range: float): + self.range = range + + @range.setter + def range(self, range: float): + wb.wb_emitter_set_range(self._tag, ctypes.c_double(range)) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/field.py b/webots_ros2_driver/webots/lib/controller/python/controller/field.py new file mode 100644 index 000000000..3b0038fec --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/field.py @@ -0,0 +1,301 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb +from controller.constants import constant +import struct +import typing + + +class Field: + SF_BOOL = constant('SF_BOOL') + SF_INT32 = constant('SF_INT32') + SF_FLOAT = constant('SF_FLOAT') + SF_VEC2F = constant('SF_VEC2F') + SF_VEC3F = constant('SF_VEC3F') + SF_ROTATION = constant('SF_ROTATION') + SF_COLOR = constant('SF_COLOR') + SF_STRING = constant('SF_STRING') + SF_NODE = constant('SF_NODE') + MF_BOOL = constant('MF_BOOL') + MF_INT32 = constant('MF_INT32') + MF_FLOAT = constant('MF_FLOAT') + MF_VEC2F = constant('MF_VEC2F') + MF_VEC3F = constant('MF_VEC3F') + MF_ROTATION = constant('MF_ROTATION') + MF_COLOR = constant('MF_COLOR') + MF_STRING = constant('MF_STRING') + MF_NODE = constant('MF_NODE') + + wb.wb_supervisor_field_get_name.restype = ctypes.c_char_p + wb.wb_supervisor_field_get_type_name.restype = ctypes.c_char_p + wb.wb_supervisor_node_get_proto_field.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_proto_field_by_index.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_field.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_field_by_index.restype = ctypes.c_void_p + wb.wb_supervisor_field_get_sf_float.restype = ctypes.c_double + wb.wb_supervisor_field_get_sf_vec2f.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_sf_vec3f.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_sf_rotation.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_sf_color.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_sf_string.restype = ctypes.c_char_p + wb.wb_supervisor_field_get_mf_float.restype = ctypes.c_double + wb.wb_supervisor_field_get_mf_vec2f.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_mf_vec3f.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_mf_rotation.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_mf_color.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_field_get_mf_string.restype = ctypes.c_char_p + wb.wb_supervisor_field_set_sf_vec2f.argtypes = [ctypes.c_void_p, ctypes.c_double * 2] + wb.wb_supervisor_field_set_sf_vec3f.argtypes = [ctypes.c_void_p, ctypes.c_double * 3] + wb.wb_supervisor_field_set_sf_rotation.argtypes = [ctypes.c_void_p, ctypes.c_double * 4] + wb.wb_supervisor_field_set_sf_color.argtypes = [ctypes.c_void_p, ctypes.c_double * 3] + wb.wb_supervisor_field_get_mf_float.restype = ctypes.c_double + wb.wb_supervisor_virtual_reality_headset_get_position = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_virtual_reality_headset_get_orientation = ctypes.POINTER(ctypes.c_double) + + def __init__(self, node, name: typing.Optional[str] = None, index: typing.Optional[int] = None, proto: bool = False): + if proto: + if name is not None: + self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_proto_field(node._ref, str.encode(name))) + else: + self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_proto_field_by_index(node._ref, index)) + else: + if name is not None: + self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_field(node._ref, str.encode(name))) + else: + self._ref = ctypes.c_void_p(wb.wb_supervisor_node_get_field_by_index(node._ref, index)) + if self._ref: + self.type = wb.wb_supervisor_field_get_type(self._ref) + + def getName(self) -> str: + return self.name + + def getType(self) -> int: + return self.type + + def getTypeName(self) -> str: + return self.type_name + + def getCount(self) -> int: + return self.count + + def enableSFTracking(self, samplingPeriod: int): + wb.wb_supervisor_field_enable_sf_tracking(samplingPeriod) + + def disableSFTracking(self): + wb.wb_supervisor_field_disable_sf_tracking() + + def getSFBool(self) -> bool: + return self.value + + def getSFInt32(self) -> int: + return self.value + + def getSFFloat(self) -> float: + return self.value + + def getSFVec2f(self) -> typing.List[float]: + return self.value + + def getSFVec3f(self) -> typing.List[float]: + return self.value + + def getSFRotation(self) -> typing.List[float]: + return self.value + + def getSFColor(self) -> typing.List[float]: + return self.value + + def getSFString(self) -> str: + return self.value + + def getMFBool(self, index: int) -> bool: + return wb.wb_supervisor_field_get_mf_bool(self._ref, index) != 0 + + def getMFInt32(self, index: int) -> int: + return wb.wb_supervisor_field_get_mf_int32(self._ref, index) + + def getMFFloat(self, index: int) -> float: + return wb.wb_supervisor_field_get_mf_float(self._ref, index) + + def getMFString(self, index: int) -> str: + return wb.wb_supervisor_field_get_mf_string(self._ref, index).decode() + + def getMFVec2f(self, index: int) -> typing.List[float]: + return wb.wb_supervisor_field_get_mf_vec2f(self._ref, index)[:2] + + def getMFVec3f(self, index: int) -> typing.List[float]: + return wb.wb_supervisor_field_get_mf_vec3f(self._ref, index)[:3] + + def getMFRotation(self, index: int) -> typing.List[float]: + return wb.wb_supervisor_field_get_mf_rotation(self._ref, index)[:4] + + def getMFColor(self, index: int) -> typing.List[float]: + return wb.wb_supervisor_field_get_mf_color(self._ref, index)[:3] + + def setSFBool(self, value: bool): + self.value = value + + def setSFInt32(self, value: int): + self.value = value + + def setSFFloat(self, value: float): + self.value = value + + def setSFVec2f(self, value: typing.List[float]): + self.value = value + + def setSFVec3f(self, value: typing.List[float]): + self.value = value + + def setSFRotation(self, value: typing.List[float]): + self.value = value + + def setSFColor(self, value: typing.List[float]): + self.value = value + + def setSFString(self, value: str): + self.value = value + + def setMFBool(self, index, value: bool): + wb.wb_supervisor_field_set_mf_bool(self._ref, index, 1 if value else 0) + + def setMFInt32(self, index, value: int): + wb.wb_supervisor_field_set_mf_int32(self._ref, index, value) + + def setMFFloat(self, index, value: float): + wb.wb_supervisor_field_set_mf_float(self._ref, index, ctypes.c_double(value)) + + def setMFVec2f(self, index, value: typing.List[float]): + data = struct.pack('dd', *value) + wb.wb_supervisor_field_set_mf_vec2f(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def setMFVec3f(self, index, value: typing.List[float]): + data = struct.pack('ddd', *value) + wb.wb_supervisor_field_set_mf_vec3f(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def setMFRotation(self, index, value: typing.List[float]): + data = struct.pack('dddd', *value) + wb.wb_supervisor_field_set_mf_rotation(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def setMFColor(self, index, value: typing.List[float]): + data = struct.pack('ddd', *value) + wb.wb_supervisor_field_set_mf_color(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def setMFString(self, index, value: str): + wb.wb_supervisor_field_set_mf_string(self._ref, index, str.encode(value)) + + def insertMFBool(self, index, value: bool): + wb.wb_supervisor_field_insert_mf_bool(self._ref, index, 1 if value else 0) + + def insertMFInt32(self, index, value: int): + wb.wb_supervisor_field_insert_mf_int32(self._ref, index, value) + + def insertMFFloat(self, index, value: float): + wb.wb_supervisor_field_insert_mf_float(self._ref, index, ctypes.c_double(value)) + + def insertMFVec2f(self, index, value: typing.List[float]): + data = struct.pack('dd', *value) + wb.wb_supervisor_field_insert_mf_vec2f(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def insertMFVec3f(self, index, value: typing.List[float]): + data = struct.pack('ddd', *value) + wb.wb_supervisor_field_insert_mf_vec3f(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def insertMFRotation(self, index, value: typing.List[float]): + data = struct.pack('dddd', *value) + wb.wb_supervisor_field_insert_mf_rotation(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def insertMFColor(self, index, value: typing.List[float]): + data = struct.pack('ddd', *value) + wb.wb_supervisor_field_insert_mf_color(self._ref, index, ctypes.POINTER(ctypes.c_ubyte(data))) + + def insertMFString(self, index, value): + wb.wb_supervisor_field_insert_mf_string(self._ref, index, str.encode(value)) + + def removeMF(self, index): + wb.wb_supervisor_field_remove_mf(self._ref, index) + + def removeSF(self): + wb.wb_supervisor_field_remove_mf(self._ref) + + def importMFNodeFromString(self, position: int, nodeString: str): + wb.wb_supervisor_field_import_mf_node_from_string(self._ref, position, str.encode(nodeString)) + + def importSFNodeFromString(self, nodeString: str): + wb.wb_supervisor_field_import_sf_node_from_string(self._ref, str.encode(nodeString)) + + def virtualRealityHeadsetIsUsed(self): + return wb.wb_supervisor_virtual_reality_headset_is_used() != 0 + + def virtualRealityHeadsetGetPosition(self) -> typing.List[float]: + return wb.wb_supervisor_virtual_reality_headset_get_position() + + def virtualRealityHeadsetGetOrientation(self) -> typing.List[float]: + return wb.wb_supervisor_virtual_reality_headset_get_orientation() + + @property + def name(self) -> str: + return wb.wb_supervisor_field_get_name(self._ref).decode() + + @property + def type_name(self) -> str: + return wb.wb_supervisor_field_get_type_name(self._ref).decode() + + @property + def count(self) -> int: + return wb.wb_supervisor_field_get_count(self._ref) + + @property + def value(self) -> typing.Union[bool, int, float, str, typing.List[float]]: + if self.type == Field.SF_BOOL: + return wb.wb_supervisor_field_get_sf_bool(self._ref) + elif self.type == Field.SF_INT32: + return wb.wb_supervisor_field_get_sf_int32(self._ref) + elif self.type == Field.SF_FLOAT: + return wb.wb_supervisor_field_get_sf_float(self._ref) + elif self.type == Field.SF_STRING: + return wb.wb_supervisor_field_get_sf_string(self._ref).decode() + elif self.type == Field.SF_VEC2F: + return wb.wb_supervisor_field_get_sf_vec2f(self._ref)[:2] + elif self.type == Field.SF_VEC3F: + return wb.wb_supervisor_field_get_sf_vec3f(self._ref)[:3] + elif self.type == Field.SF_ROTATION: + return wb.wb_supervisor_field_get_sf_rotation(self._ref)[:4] + elif self.type == Field.SF_COLOR: + return wb.wb_supervisor_field_get_sf_color(self._ref)[:3] + elif self.type == Field.SF_NODE: + return self.getSFNode() + else: + return None + + @value.setter + def value(self, value: typing.Union[bool, int, float, str, typing.List[float]]): + if self.type == Field.SF_BOOL and isinstance(value, bool): + wb.wb_supervisor_field_set_sf_bool(self._ref, 1 if value else 0) + elif self.type == Field.SF_INT32 and isinstance(value, int): + wb.wb_supervisor_field_set_sf_int32(self._ref, value) + elif self.type == Field.SF_FLOAT and isinstance(value, float): + wb.wb_supervisor_field_set_sf_float(self._ref, ctypes.c_double(value)) + elif self.type == Field.SF_STRING and isinstance(value, str): + wb.wb_supervisor_field_set_sf_string(self._ref, str.encode(value)) + elif self.type == Field.SF_VEC2F and isinstance(value, list) and len(value) == 2: + wb.wb_supervisor_field_set_sf_vec2f(self._ref, (ctypes.c_double * 2)(*value)) + elif self.type == Field.SF_VEC3F and isinstance(value, list) and len(value) == 3: + wb.wb_supervisor_field_set_sf_vec3f(self._ref, (ctypes.c_double * 3)(*value)) + elif self.type == Field.SF_ROTATION and isinstance(value, list) and len(value) == 4: + wb.wb_supervisor_field_set_sf_rotation(self._ref, (ctypes.c_double * 4)(*value)) + elif self.type == Field.SF_COLOR and isinstance(value, list) and len(value) == 3: + wb.wb_supervisor_field_set_sf_color(self._ref, (ctypes.c_double * 3)(*value)) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/gps.py b/webots_ros2_driver/webots/lib/controller/python/controller/gps.py new file mode 100644 index 000000000..0b8901820 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/gps.py @@ -0,0 +1,68 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class GPS(Sensor): + LOCAL = 0 + WGS84 = 1 + + wb.wb_gps_get_speed.restype = ctypes.c_double + wb.wb_gps_get_speed_vector.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_gps_get_values.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_gps_convert_to_degrees_minutes_seconds.restype = ctypes.c_char_p + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_gps_enable + self._get_sampling_period = wb.wb_gps_get_sampling_period + super().__init__(name, sampling_period) + + def getCoordinateSystem(self) -> int: + return self.coordinate_system + + @property + def coordinate_system(self) -> int: + return wb.wb_gps_get_coordinate_system(self._tag) + + @staticmethod + def convertToDegreesMinutesSeconds(decimalDegrees) -> str: + degrees = int(decimalDegrees) + minutes = int(decimalDegrees - degrees) * 60 + seconds = int(((decimalDegrees - degrees) * 60) - minutes) * 60 + return f'{degrees}° {minutes}′ {seconds}″' + + def getSpeed(self) -> float: + return self.speed + + @property + def speed(self) -> float: + return wb.wb_gps_get_speed(self._tag) + + def getSpeedVector(self) -> typing.List[float]: + return self.speed_vector + + @property + def speed_vector(self) -> typing.List[float]: + return wb.wb_gps_get_speed_vector(self._tag)[:3] + + def getValues(self) -> typing.List[float]: + return self.value + + @property + def value(self) -> typing.List[float]: + return wb.wb_gps_get_values(self._tag)[:3] diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/gyro.py b/webots_ros2_driver/webots/lib/controller/python/controller/gyro.py new file mode 100644 index 000000000..2735090ed --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/gyro.py @@ -0,0 +1,43 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class Gyro(Sensor): + wb.wb_gyro_get_values.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_gyro_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_gyro_enable + self._get_sampling_period = wb.wb_gyro_get_sampling_period + super().__init__(name, sampling_period) + + def getValues(self) -> typing.List[float]: + return self.value + + def getLookupTable(self) -> typing.List[float]: + return self.lookup_table + + @property + def lookup_table(self) -> typing.List[float]: + size = wb.wb_gyro_get_lookup_table_size(self._tag) + return wb.wb_gyro_get_lookup_table(self._tag)[: 3 * size] + + @property + def value(self) -> typing.List[float]: + return wb.wb_gyro_get_values(self._tag)[:3] diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/inertial_unit.py b/webots_ros2_driver/webots/lib/controller/python/controller/inertial_unit.py new file mode 100644 index 000000000..872bccf7e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/inertial_unit.py @@ -0,0 +1,50 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class InertialUnit(Sensor): + wb.wb_inertial_unit_get_roll_pitch_yaw.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_inertial_unit_get_quaternion.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_inertial_unit_get_noise.restype = ctypes.c_double + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_inertial_unit_enable + self._get_sampling_period = wb.wb_inertial_unit_get_sampling_period + super().__init__(name, sampling_period) + + def getRollPitchYaw(self) -> typing.List[float]: + return self.roll_pitch_yaw + + def getQuaternion(self) -> typing.List[float]: + return self.quaternion + + def getNoise(self) -> float: + return self.noise + + @property + def roll_pitch_yaw(self) -> typing.List[float]: + return wb.wb_inertial_unit_get_roll_pitch_yaw(self._tag)[:3] + + @property + def quaternion(self) -> typing.List[float]: + return wb.wb_inertial_unit_get_quaternion(self._tag)[:4] + + @property + def noise(self) -> float: + return wb.wb_inertial_unit_get_noise(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/joystick.py b/webots_ros2_driver/webots/lib/controller/python/controller/joystick.py new file mode 100644 index 000000000..851ce13bc --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/joystick.py @@ -0,0 +1,97 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +import ctypes +from typing import Union + + +class Joystick(): + wb.wb_joystick_get_model.restype = ctypes.c_char_p + + def __init__(self, sampling_period: int = None): + if sampling_period != 0: + self.sampling_period = int(wb.wb_robot_get_basic_time_step()) if sampling_period is None else sampling_period + + def enable(self, p: int): + wb.wb_joystick_enable(p) + + def disable(self): + wb.wb_joystick_disable() + + def getSamplingPeriod(self) -> int: + return self.sampling_period + + def isConnected(self) -> bool: + return self.is_connected + + def getNumberOfAxes(self) -> int: + return self.number_of_axes + + def getAxisValue(self, axis: int) -> float: + return wb.wb_joystick_get_axis_value(axis) + + def getNumberOfPovs(self) -> int: + return self.number_of_povs + + def getPovValue(self, pov: int) -> int: + return wb.wb_joystick_get_pov_value(pov) + + def getPressedButton(self) -> int: + return self.pressed_button + + def setConstantForce(self, level: int): + wb.wb_joystick_set_constant_force(level) + + def setConstantForceDuration(self, duration: float): + wb.wb_joystick_set_constant_force_duration(ctypes.c_double(duration)) + + def setAutoCenteringGain(self, gain: float): + wb.wb_joystick_set_auto_centering_gain(ctypes.c_double(gain)) + + def setResistanceGain(self, gain: float): + wb.wb_joystick_set_resistance_gain(ctypes.c_double(gain)) + + def setForceAxis(self, axis: int): + wb.wb_joystick_set_force_axis(axis) + + @property + def sampling_period(self) -> int: + return wb.wb_joystick_get_sampling_period() + + @sampling_period.setter + def sampling_period(self, p: Union[int, None]): + if p is None: + p = 0 + wb.wb_joystick_enable(p) + + @property + def is_connected(self) -> bool: + return wb.wb_joystick_is_connected() != 0 + + @property + def model(self) -> str: + return wb.wb_joystick_get_model().decode() + + @property + def number_of_axes(self) -> int: + return wb.wb_joystick_get_number_of_axes() + + @property + def number_of_povs(self) -> int: + return wb.wb_joystick_get_number_of_povs() + + @property + def pressed_button(self) -> int: + return wb.wb_joystick_get_pressed_button() diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/keyboard.py b/webots_ros2_driver/webots/lib/controller/python/controller/keyboard.py new file mode 100644 index 000000000..d607afd3f --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/keyboard.py @@ -0,0 +1,110 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.constants import constant +from typing import Union + + +class Keyboard(): + END = constant('KEYBOARD_END') + HOME = constant('KEYBOARD_HOME') + LEFT = constant('KEYBOARD_LEFT') + UP = constant('KEYBOARD_UP') + RIGHT = constant('KEYBOARD_RIGHT') + DOWN = constant('KEYBOARD_DOWN') + PAGEUP = constant('KEYBOARD_PAGEUP') + PAGEDOWN = constant('KEYBOARD_PAGEDOWN') + NUMPAD_HOME = constant('KEYBOARD_NUMPAD_HOME') + NUMPAD_LEFT = constant('KEYBOARD_NUMPAD_LEFT') + NUMPAD_UP = constant('KEYBOARD_NUMPAD_UP') + NUMPAD_RIGHT = constant('KEYBOARD_NUMPAD_RIGHT') + NUMPAD_DOWN = constant('KEYBOARD_NUMPAD_DOWN') + NUMPAD_END = constant('KEYBOARD_NUMPAD_END') + KEY = constant('KEYBOARD_KEY') + SHIFT = constant('KEYBOARD_SHIFT') + CONTROL = constant('KEYBOARD_CONTROL') + ALT = constant('KEYBOARD_ALT') + + def __init__(self, sampling_period: int = None): + if sampling_period != 0: + self.sampling_period = int(wb.wb_robot_get_basic_time_step()) if sampling_period is None else sampling_period + + def enable(self, p: int): + wb.wb_keyboard_enable(p) + + def disable(self): + wb.wb_keyboard_disable() + + def getSamplingPeriod(self) -> int: + return self.sampling_period + + @property + def sampling_period(self) -> int: + return wb.wb_keyboard_get_sampling_period() + + @sampling_period.setter + def sampling_period(self, p: Union[int, None]): + if p is None: + p = 0 + wb.wb_keyboard_enable(p) + + def getKeyCode(self) -> int: + return wb.wb_keyboard_get_key() + + def getKey(self) -> int: + k = wb.wb_keyboard_get_key() + return k + + def get_key(self) -> str: + k = wb.wb_keyboard_get_key() + s = '' + if k & Keyboard.SHIFT != 0: + s += 'shift-' + if k & Keyboard.CONTROL != 0: + s += 'control-' + if k & Keyboard.ALT != 0: + s += 'atl-' + k &= Keyboard.KEY + if k == Keyboard.END: + s += 'end' + elif k == Keyboard.HOME: + s += 'home' + elif k == Keyboard.LEFT: + s += 'left' + elif k == Keyboard.RIGHT: + s += 'right' + elif k == Keyboard.UP: + s += 'up' + elif k == Keyboard.DOWN: + s += 'down' + elif k == Keyboard.PAGEUP: + s += 'page up' + elif k == Keyboard.PAGEDOWN: + s += 'page down' + elif k == Keyboard.NUMPAD_END: + s += 'numpad end' + elif k == Keyboard.NUMPAD_HOME: + s += 'numpad home' + elif k == Keyboard.NUMPAD_LEFT: + s += 'numpad left' + elif k == Keyboard.NUMPAD_RIGHT: + s += 'numpad right' + elif k == Keyboard.NUMPAD_UP: + s += 'numpad up' + elif k == Keyboard.NUMPAD_DOWN: + s += 'numpad down' + else: + s += chr(k) + return s diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/led.py b/webots_ros2_driver/webots/lib/controller/python/controller/led.py new file mode 100644 index 000000000..7380fe5d3 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/led.py @@ -0,0 +1,39 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.device import Device +from typing import Union + + +class LED(Device): + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def set(self, v: Union[bool, int]): + self.value = v + + def get(self) -> int: + return self.value + + @property + def value(self) -> int: + return wb.wb_led_get(self._tag) + + @value.setter + def value(self, v: Union[bool, int]): + if isinstance(v, bool): + wb.wb_led_set(self._tag, 1 if v else 0) + else: + wb.wb_led_set(self._tag, v) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/lidar.py b/webots_ros2_driver/webots/lib/controller/python/controller/lidar.py new file mode 100644 index 000000000..76f39f2ae --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/lidar.py @@ -0,0 +1,156 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.sensor import Sensor +from controller.lidar_point import LidarPoint +from controller.wb import wb +from typing import List, Union + + +class Lidar(Sensor): + wb.wb_lidar_get_fov.restype = ctypes.c_double + wb.wb_lidar_get_frequency.restype = ctypes.c_double + wb.wb_lidar_get_vertical_fov.restype = ctypes.c_double + wb.wb_lidar_get_max_frequency.restype = ctypes.c_double + wb.wb_lidar_get_min_frequency.restype = ctypes.c_double + wb.wb_lidar_get_max_range.restype = ctypes.c_double + wb.wb_lidar_get_min_range.restype = ctypes.c_double + wb.wb_lidar_get_point_cloud.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_lidar_get_layer_point_cloud.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_lidar_get_range_image.restype = ctypes.POINTER(ctypes.c_float) + wb.wb_lidar_get_layer_range_image.restype = ctypes.POINTER(ctypes.c_float) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_lidar_enable + self._get_sampling_period = wb.wb_lidar_get_sampling_period + super().__init__(name, sampling_period) + + def getFov(self) -> float: + return self.fov + + def getVerticalFov(self) -> float: + return self.vertical_fov + + def getMaxFrequency(self) -> float: + return self.max_frequency + + def getMinFrequency(self) -> float: + return self.min_frequency + + def getMaxRange(self) -> float: + return self.max_range + + def getMinRange(self) -> float: + return self.min_range + + def getHorizontalResolution(self) -> int: + return self.horizontal_resolution + + def getNumberOfLayers(self) -> int: + return self.number_of_layers + + def getFrequency(self) -> float: + return self.frequency + + def setFrequency(self, frequency: float): + self.frequency = frequency + + def getNumberOfPoints(self) -> int: + return self.number_of_points + + def getRangeImage(self) -> List[float]: + return self.range_image[:self.horizontal_resolution * self.number_of_layers] + + def defRangeImageArray(self) -> List[List[float]]: + array = [] + for i in range(self.number_of_layers): + array.append(self.getLayerRangeImage(i)) + return array + + def getLayerRangeImage(self, layer) -> List[float]: + return wb.wb_lidar_get_range_image(self._tag, layer)[:self.horizontal_resolution] + + @property + def range_image(self): + return wb.wb_lidar_get_range_image(self._tag) + + @property + def fov(self) -> float: + return wb.wb_lidar_get_fov(self._tag) + + @property + def vertical_fov(self) -> float: + return wb.wb_lidar_get_vertical_fov(self._tag) + + @property + def max_frequency(self) -> float: + return wb.wb_lidar_get_max_frequency(self._tag) + + @property + def min_frequency(self) -> float: + return wb.wb_lidar_get_min_frequency(self._tag) + + @property + def max_range(self) -> float: + return wb.wb_lidar_get_max_range(self._tag) + + @property + def min_range(self) -> float: + return wb.wb_lidar_get_min_range(self._tag) + + @property + def horizontal_resolution(self) -> int: + return wb.wb_lidar_get_horizontal_resolution(self._tag) + + @property + def number_of_layers(self) -> int: + return wb.wb_lidar_get_number_of_layers(self._tag) + + @property + def frequency(self) -> float: + return wb.wb_lidar_get_frequency(self._tag) + + @frequency.setter + def frequency(self, frequency: float): + wb.wb_lidar_set_frequency(self._tag, ctypes.c_double(frequency)) + + @property + def number_of_points(self) -> int: + return wb.wb_lidar_get_number_of_points(self._tag) + + def disablePointCloud(self): + wb.wb_lidar_disable_point_cloud(self._tag) + + def enablePointCloud(self): + wb.wb_lidar_enable_point_cloud(self._tag) + + def isPointCloudEnabled(self) -> bool: + return wb.wb_lidar_is_point_cloud_enabled(self._tag) != 0 + + def getPointCloud(self) -> List[LidarPoint]: + number_of_points = self.number_of_points + data = bytes(wb.wb_lidar_get_point_cloud(self._tag)[:number_of_points * 20]) + list = [] + for i in range(number_of_points): + list.append(LidarPoint(data, i)) + return list + + def getLayerPointCloud(self, layer: int) -> List[LidarPoint]: + number_of_points = int(self.number_of_points / self.number_of_layers) + data = bytes(wb.wb_lidar_get_layer_point_cloud(self._tag, layer)[:number_of_points * 20]) + list = [] + for i in range(number_of_points): + list.append(LidarPoint(data, i)) + return list diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/lidar_point.py b/webots_ros2_driver/webots/lib/controller/python/controller/lidar_point.py new file mode 100644 index 000000000..d922d4b30 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/lidar_point.py @@ -0,0 +1,25 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import struct + + +class LidarPoint: + def __init__(self, data: bytes, offset): + t = struct.unpack_from('fffif', data, offset * 20) + self.x = t[0] + self.y = t[1] + self.z = t[2] + self.layer = t[3] + self.time = t[4] diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/light_sensor.py b/webots_ros2_driver/webots/lib/controller/python/controller/light_sensor.py new file mode 100644 index 000000000..f1fb7e413 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/light_sensor.py @@ -0,0 +1,42 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb +from controller.sensor import Sensor +from typing import Union, List + + +class LightSensor(Sensor): + wb.wb_light_sensor_get_value.restype = ctypes.c_double + wb.wb_light_sensor_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_light_sensor_enable + self._get_sampling_period = wb.wb_light_sensor_get_sampling_period + super().__init__(name, sampling_period) + + def getLookupTable(self) -> List[float]: + return self.lookup_table[:3 * wb.wb_light_sensor_get_lookup_table_size(self._tag)] + + def getValue(self) -> float: + return self.value + + @property + def lookup_table(self): + return wb.wb_light_sensor_get_lookup_table(self._tag) + + @property + def value(self) -> float: + return wb.wb_light_sensor_get_value(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/motion.py b/webots_ros2_driver/webots/lib/controller/python/controller/motion.py new file mode 100644 index 000000000..3824c5591 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/motion.py @@ -0,0 +1,65 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb + + +class Motion: + wb.wbu_motion_new.restype = ctypes.c_void_p + + def __init__(self, filename: str): + self._ref = ctypes.c_void_p(wb.wbu_motion_new(str.encode(filename))) + + def __del__(self): + wb.wbu_motion_delete(self._ref) + + def isValid(self): + return self._ref != ctypes.c_void_p(0) + + def play(self): + wb.wbu_motion_play(self._ref) + + def stop(self): + wb.wbu_motion_stop(self._ref) + + def setLoop(self, loop: bool): + wb.wbu_motion_set_loop(self._ref, 1 if loop else 0) + + def setReverse(self, reverse: bool): + wb.wbu_motion_set_reverse(self._ref, 1 if reverse else 0) + + def isOver(self) -> bool: + return wb.wbu_motion_is_over(self._ref) != 0 + + def getDuration(self) -> int: + return self.duration + + def getTime(self) -> int: + return self.time + + def setTime(self, time: int): + self.time = time + + @property + def time(self) -> int: + return wb.wbu_motion_get_time(self._ref) + + @time.setter + def time(self, time): + wb.wbu_motion_set_time(self._ref, time) + + @property + def duration(self) -> int: + return wb.wbu_motion_get_duration(self._ref) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/motor.py b/webots_ros2_driver/webots/lib/controller/python/controller/motor.py new file mode 100644 index 000000000..f5b0b19ae --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/motor.py @@ -0,0 +1,238 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.constants import constant +from controller.device import Device +from controller.wb import wb +from typing import Union + + +class Motor(Device): + wb.wb_motor_get_target_position.restype = ctypes.c_double + wb.wb_motor_get_max_position.restype = ctypes.c_double + wb.wb_motor_get_min_position.restype = ctypes.c_double + wb.wb_motor_get_velocity.restype = ctypes.c_double + wb.wb_motor_get_max_velocity.restype = ctypes.c_double + wb.wb_motor_get_acceleration.restype = ctypes.c_double + wb.wb_motor_get_available_force.restype = ctypes.c_double + wb.wb_motor_get_available_torque.restype = ctypes.c_double + wb.wb_motor_get_max_force.restype = ctypes.c_double + wb.wb_motor_get_max_torque.restype = ctypes.c_double + wb.wb_motor_get_multiplier.restype = ctypes.c_double + wb.wb_motor_get_force_feedback.restype = ctypes.c_double + wb.wb_motor_get_torque_feedback.restype = ctypes.c_double + + ROTATIONAL = constant('ROTATIONAL') + LINEAR = constant('LINEAR') + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def setPosition(self, position: float): + self.target_position = position + + def setVelocity(self, velocity: float): + self.target_velocity = velocity + + def setAcceleration(self, acceleration: float): + self.target_acceleration = acceleration + + def setAvailableForce(self, force: float): + self.available_force = force + + def setAvailableTorque(self, torque: float): + self.available_torque = torque + + def setControlPID(self, p: float, i: float, d: float): + wb.wb_motor_set_control_pid(self._tag, ctypes.c_double(p), ctypes.c_double(i), ctypes.c_double(d)) + + def getTargetPosition(self) -> float: + return self.target_position + + def getMinPosition(self) -> float: + return self.min_position + + def getMaxPosition(self) -> float: + return self.max_position + + def getVelocity(self) -> float: + return self.target_velocity + + def getMaxVelocity(self) -> float: + return self.max_velocity + + def getAcceleration(self) -> float: + return self.target_acceleration + + def getAvailableForce(self) -> float: + return self.available_force + + def getMaxForce(self) -> float: + return self.max_force + + def getAvailableTorque(self) -> float: + return self.available_torque + + def getMaxTorque(self) -> float: + return self.max_torque + + def getMultiplier(self) -> float: + return self.multiplier + + def enableForceFeedback(self): + wb.wb_motor_enable_force_feedback(self._tag) + + def disableForceFeedback(self): + wb.wb_motor_disable_force_feedback(self._tag) + + def getForceFeedbackSamplingPeriod(self) -> int: + return self.force_feedback_sampling_period + + def getForceFeedback(self) -> float: + return self.force_feedback + + def enableTorqueFeedback(self, sampling_period: int): + wb.wb_motor_enable_torque_feedback(self._tag, sampling_period) + + def disableTorqueFeedback(self): + wb.wb_motor_disable_torque_feedback(self._tag) + + def getTorqueFeedbackSamplingPeriod(self) -> int: + return self.torque_feedback_sampling_period + + def getTorqueFeedback(self) -> float: + return self.torque_feedback + + def setForce(self, force: float): + wb.wb_motor_set_force(self._tag, ctypes.c_double(force)) + + def setTorque(self, torque: float): + wb.wb_motor_set_torque(self._tag, ctypes.c_double(torque)) + + def getType(self) -> int: + return wb.wb_motor_get_type(self._tag) + + def getBrake(self): + return self.brake + + def getPositionSensor(self): + return self.position_sensor + + @property + def brake(self): + from controller.brake import Brake + tag = wb.wb_motor_get_brake(self._tag) + return None if tag == 0 else Brake(tag) + + @property + def position_sensor(self): + from controller.position_sensor import PositionSensor + tag = wb.wb_motor_get_position_sensor(self._tag) + return None if tag == 0 else PositionSensor(tag) + + @property + def force_feedback_sampling_period(self) -> int: + return wb.wb_motor_get_force_feedback_sampling_period(self._tag) + + @force_feedback_sampling_period.setter + def force_feedback_sampling_period(self, sampling_period): + wb.wb_motor_enable_force_feedback(self._tag, sampling_period) + + @property + def torque_feedback_sampling_period(self) -> int: + return wb.wb_motor_get_torque_feedback_sampling_period(self._tag) + + @torque_feedback_sampling_period.setter + def torque_feedback_sampling_period(self, sampling_period): + wb.wb_motor_enable_torque_feedback(self._tag, sampling_period) + + @property + def max_position(self) -> float: + return wb.wb_motor_get_max_position(self._tag) + + @property + def min_position(self) -> float: + return wb.wb_motor_get_min_position(self._tag) + + @property + def max_velocity(self) -> float: + return wb.wb_motor_get_max_velocity(self._tag) + + @property + def target_position(self) -> float: + return wb.wb_motor_get_target_position(self._tag) + + @target_position.setter + def target_position(self, position: float): + wb.wb_motor_set_position(self._tag, ctypes.c_double(position)) + + @property + def target_velocity(self) -> float: + return wb.wb_motor_get_velocity(self._tag) + + @target_velocity.setter + def target_velocity(self, velocity: float): + wb.wb_motor_set_velocity(self._tag, ctypes.c_double(velocity)) + + @property + def available_force(self) -> float: + return wb.wb_motor_get_available_force(self._tag) + + @available_force.setter + def available_force(self, force: float): + wb.wb_motor_set_available_force(self._tag, ctypes.c_double(force)) + + @property + def max_force(self) -> float: + return wb.wb_motor_get_max_force(self._tag) + + @property + def available_torque(self) -> float: + return wb.wb_motor_get_available_torque(self._tag) + + @available_torque.setter + def available_torque(self, torque: float): + wb.wb_motor_set_available_torque(self._tag, ctypes.c_double(torque)) + + @property + def max_torque(self) -> float: + return wb.wb_motor_get_max_torque(self._tag) + + @property + def target_acceleration(self) -> float: + return wb.wb_motor_get_acceleration(self._tag) + + @target_acceleration.setter + def target_acceleration(self, acceleration: float): + wb.wb_motor_set_acceleration(acceleration) + + force = property(fset=setForce) + torque = property(fset=setTorque) + + @property + def multiplier(self) -> float: + return wb.wb_motor_get_multiplier(self._tag) + + @property + def force_feedback(self) -> float: + return wb.wb_motor_get_force_feedback(self._tag) + + @property + def torque_feedback(self) -> float: + return wb.wb_motor_get_torque_feedback(self._tag) + + @property + def type(self) -> int: + return wb.wb_motor_get_type(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/mouse.py b/webots_ros2_driver/webots/lib/controller/python/controller/mouse.py new file mode 100644 index 000000000..c9e92ddd4 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/mouse.py @@ -0,0 +1,71 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +import ctypes +import struct +from typing import Union + + +class MouseState(): + def __init__(self, u: float, v: float, x: float, y: float, z: float, left: bool, middle: bool, right: bool): + self.u = u + self.v = v + self.x = x + self.y = y + self.z = z + self.left = left + self.middle = middle + self.right = right + + +class Mouse(): + wb.wb_mouse_get_state_pointer.restype = ctypes.POINTER(ctypes.c_ubyte) + + def __init__(self, sampling_period: int = None): + if sampling_period != 0: + self.sampling_period = int(wb.wb_robot_get_basic_time_step()) if sampling_period is None else sampling_period + + def enable(self, p: int): + wb.wb_mouse_enable(p) + + def disable(self): + wb.wb_mouse_disable() + + def getSamplingPeriod(self) -> int: + return self.sampling_period + + def getState(self) -> MouseState: + data = bytes(wb.wb_mouse_get_state_pointer()[0:43]) + values = struct.unpack('5d3?', data) + return MouseState(values[0], values[1], values[2], values[3], values[4], values[5], values[6], values[7]) + + def enable3dPosition(self): + wb.wb_mouse_enable_3d_position() + + def disable3dPosition(self): + wb.wb_mouse_disable_3d_position() + + def is3dPositionEnabled(self) -> bool: + return wb.wb_mouse_is_3d_position_enabled() != 0 + + @property + def sampling_period(self) -> int: + return wb.wb_mouse_get_sampling_period() + + @sampling_period.setter + def sampling_period(self, p: Union[int, None]): + if p is None: + p = 0 + wb.wb_mouse_enable(p) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/node.py b/webots_ros2_driver/webots/lib/controller/python/controller/node.py new file mode 100644 index 000000000..a6f416ef1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/node.py @@ -0,0 +1,327 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.wb import wb +from controller.constants import constant +from controller import Field +import struct +import typing + + +class ContactPoint: + def __init__(self, point): + self.point = point[0:3] + self.node_id = point[3] + + def getPoint(self): + return self.point + + def getNodeId(self): + return self.node_id + + +class Node: + pass + + +class Node: + wb.wb_supervisor_node_get_root.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_selected.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_from_def.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_self.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_from_device.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_from_id.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_parent_node.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_from_proto_def.restype = ctypes.c_void_p + wb.wb_supervisor_node_get_type_name.restype = ctypes.c_char_p + wb.wb_supervisor_node_get_base_type_name.restype = ctypes.c_char_p + wb.wb_supervisor_node_export_string.restype = ctypes.c_char_p + wb.wb_supervisor_node_get_def.restype = ctypes.c_char_p + wb.wb_supervisor_node_get_position.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_node_get_orientation.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_node_get_pose.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_node_get_center_of_mass.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_supervisor_node_get_contact_points.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_supervisor_node_get_velocity.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, DEF: typing.Optional[str] = None, tag: typing.Optional[int] = None, id: typing.Optional[int] = None, + selected: typing.Optional[bool] = None, ref: typing.Optional[int] = None): + if ref is None: + if id is None: + if tag is None: + if DEF is None: + if selected is None: + ref = wb.wb_supervisor_node_get_root() + else: + ref = wb.wb_supervisor_node_get_selected() + else: + ref = wb.wb_supervisor_node_get_from_def(str.encode(DEF)) + else: + if tag == 0: + ref = wb.wb_supervisor_node_get_self() + else: + ref = wb.wb_supervisor_node_get_from_device(tag) + else: + ref = wb.wb_supervisor_node_get_from_id(id) + self._ref = ctypes.c_void_p(ref) + + def getDef(self) -> str: + return self.DEF + + def getId(self) -> int: + return self.id + + def getParentNode(self) -> Node: + return Node(ref=wb.wb_supervisor_node_get_parent_node(self._ref)) + + def isProto(self) -> bool: + return wb.wb_supervisor_node_is_proto(self._ref) != 0 + + def getFromProtoDef(self, DEF: str) -> Node: + return Node(wb.wb_supervisor_node_get_from_proto_def(self._ref, str.encode(DEF))) + + def getType(self) -> int: + return self.type + + def getTypeName(self) -> str: + return self.type_name + + def getBaseTypeName(self) -> str: + return self.base_type_name + + def remove(self): + wb.wb_supervisor_node_remove(self._ref) + + def exportString(self): + return wb.wb_supervisor_node_export_string(self._ref).decode() + + def getField(self, name: str) -> Field: + return Field(self, name=name) + + def getFieldByIndex(self, index: int) -> Field: + return Field(self, index=index) + + def getNumberOfFields(self) -> int: + return self.number_of_fields + + def getProtoField(self, name: str) -> Field: + return Field(self, name=name, proto=True) + + def getProtoFieldByIndex(self, index: int) -> Field: + return Field(self, index=index, proto=True) + + def getPosition(self) -> typing.List[float]: + p = wb.wb_supervisor_node_get_position(self._ref) + return [p[0], p[1], p[2]] + + def getOrientation(self) -> typing.List[float]: + o = wb.wb_supervisor_node_get_orientation(self._ref) + return [o[0], o[1], o[2], o[3], o[4], o[5], o[6], o[7], o[8]] + + def getPose(self, fromNode: Node) -> typing.List[float]: + p = wb.wb_supervisor_node_get_pose(self._ref, fromNode._ref) + return [p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14], p[15]] + + def enablePoseTracking(self, samplingPeriod: int, fromNode: Node): + wb.wb_supervisor_node_enable_pose_tracking(samplingPeriod, self._ref, fromNode._ref) + + def disablePoseTracking(self, fromNode: Node): + wb.wb_supervisor_node_enable_pose_tracking(self._ref, fromNode._ref) + + def getCenterOfMass(self) -> typing.List[float]: + c = wb.wb_supervisor_node_get_center_of_mass(self._ref) + return [c[0], c[1], c[2]] + + def getContactPoints(self, includeDescendants: bool = False) -> typing.List[ContactPoint]: + size = ctypes.c_int(0) + p = wb.wb_supervisor_node_get_contact_points(self._ref, 1 if includeDescendants else 0, ctypes.byref(size)) + points = bytes(p[0:size.value * 28]) + contact_points = [] + for i in range(size.value): + contact_points.append(ContactPoint(struct.unpack_from('3di', points, 28 * i))) + return contact_points + + def enableContactPointTracking(self, samplingPeriod: int, includeDescendants: bool = False): + wb.wb_supervisor_node_enable_contact_point_tracking(samplingPeriod, 1 if includeDescendants else 0) + + def disableContactPointTracking(self, includeDescendants: bool = False): + wb.wb_supervisor_node_disable_contact_point_tracking(1 if includeDescendants else 0) + + def getStaticBalance(self) -> bool: + return wb.wb_supervisor_node_get_static_balance(self._ref) != 0 + + def getVelocity(self) -> typing.List[float]: + v = wb.wb_supervisor_node_get_velocity(self._ref) + return [v[0], v[1], v[2], v[3], v[4], v[5]] + + def setVelocity(self, velocity: typing.List[float]): + wb.wb_supervisor_node_set_velocity(self._ref, (ctypes.c_double * 6)(*velocity)) + + def saveState(self, stateName: str): + wb.wb_supervisor_node_save_state(self._ref, str.encode(stateName)) + + def loadState(self, stateName: str): + wb.wb_supervisor_node_load_state(self._ref, str.encode(stateName)) + + def resetPhysics(self): + wb.wb_supervisor_node_reset_physics(self._ref) + + def setJointPosition(self, position: float, index: int = 1): + wb.wb_supervisor_node_set_joint_position(self._ref, ctypes.c_double(position), index) + + def restartController(self): + wb.wb_supervisor_node_restart_controller(self._ref) + + def moveViewpoint(self): + wb.wb_supervisor_node_move_viewpoint(self._ref) + + def setVisibility(self, fromNode: Node, visible: bool): + wb.wb_supervisor_node_set_visibility(self._ref, fromNode._ref, 1 if visible else 0) + + def addForce(self, force: typing.List[float], relative: bool): + wb.wb_supervisor_node_add_force(self._ref, (ctypes.c_double * 3)(*force), 1 if relative else 0) + + def addForceWithOffset(self, force: typing.List[float], offset: typing.List[float], relative: bool): + wb.wb_supervisor_node_add_force(self._ref, (ctypes.c_double * 3)(*force), (ctypes.c_double * 3)(*offset), + 1 if relative else 0) + + def addTorque(self, torque: typing.List[float], relative: bool): + wb.wb_supervisor_node_add_force(self._ref, (ctypes.c_double * 4)(*torque), 1 if relative else 0) + + @property + def DEF(self) -> str: + return wb.wb_supervisor_node_get_def(self._ref).decode() + + @property + def id(self) -> int: + return wb.wb_supervisor_node_get_id(self._ref) + + @property + def type(self) -> int: + return wb.wb_supervisor_node_get_type(self._ref) + + @property + def type_name(self) -> str: + return wb.wb_supervisor_node_get_type_name(self._ref).decode() + + @property + def base_type_name(self) -> str: + return wb.wb_supervisor_node_get_base_type_name(self._ref).decode() + + @property + def number_of_fields(self) -> int: + return wb.wb_supervisor_node_get_number_of_fields(self._ref) + + +Node.NO_NODE = constant('NODE_NO_NODE') +Node.APPEARANCE = constant('NODE_APPEARANCE') +Node.BACKGROUND = constant('NODE_BACKGROUND') +Node.BILLBOARD = constant('NODE_BILLBOARD') +Node.BOX = constant('NODE_BOX') +Node.CAD_SHAPE = constant('NODE_CAD_SHAPE') +Node.CAPSULE = constant('NODE_CAPSULE') +Node.COLOR = constant('NODE_COLOR') +Node.CONE = constant('NODE_CONE') +Node.COORDINATE = constant('NODE_COORDINATE') +Node.CYLINDER = constant('NODE_CYLINDER') +Node.DIRECTIONAL_LIGHT = constant('NODE_DIRECTIONAL_LIGHT') +Node.ELEVATION_GRID = constant('NODE_ELEVATION_GRID') +Node.FOG = constant('NODE_FOG') +Node.GROUP = constant('NODE_GROUP') +Node.IMAGE_TEXTURE = constant('NODE_IMAGE_TEXTURE') +Node.INDEXED_FACE_SET = constant('NODE_INDEXED_FACE_SET') +Node.INDEXED_LINE_SET = constant('NODE_INDEXED_LINE_SET') +Node.MATERIAL = constant('NODE_MATERIAL') +Node.MESH = constant('NODE_MESH') +Node.MUSCLE = constant('NODE_MUSCLE') +Node.NORMAL = constant('NODE_NORMAL') +Node.PBR_APPEARANCE = constant('NODE_PBR_APPEARANCE') +Node.PLANE = constant('NODE_PLANE') +Node.POINT_LIGHT = constant('NODE_POINT_LIGHT') +Node.POINT_SET = constant('NODE_POINT_SET') +Node.SHAPE = constant('NODE_SHAPE') +Node.SPHERE = constant('NODE_SPHERE') +Node.SPOT_LIGHT = constant('NODE_SPOT_LIGHT') +Node.TEXTURE_COORDINATE = constant('NODE_TEXTURE_COORDINATE') +Node.TEXTURE_TRANSFORM = constant('NODE_TEXTURE_TRANSFORM') +Node.TRANSFORM = constant('NODE_TRANSFORM') +Node.VIEWPOINT = constant('NODE_VIEWPOINT') +Node.ROBOT = constant('NODE_ROBOT') +Node.ACCELEROMETER = constant('NODE_ACCELEROMETER') +Node.ALTIMETER = constant('NODE_ALTIMETER') +Node.BRAKE = constant('NODE_BRAKE') +Node.CAMERA = constant('NODE_CAMERA') +Node.COMPASS = constant('NODE_COMPASS') +Node.CONNECTOR = constant('NODE_CONNECTOR') +Node.DISPLAY = constant('NODE_DISPLAY') +Node.DISTANCE_SENSOR = constant('NODE_DISTANCE_SENSOR') +Node.EMITTER = constant('NODE_EMITTER') +Node.GPS = constant('NODE_GPS') +Node.GYRO = constant('NODE_GYRO') +Node.INERTIAL_UNIT = constant('NODE_INERTIAL_UNIT') +Node.LED = constant('NODE_LED') +Node.LIDAR = constant('NODE_LIDAR') +Node.LIGHT_SENSOR = constant('NODE_LIGHT_SENSOR') +Node.LINEAR_MOTOR = constant('NODE_LINEAR_MOTOR') +Node.PEN = constant('NODE_PEN') +Node.POSITION_SENSOR = constant('NODE_POSITION_SENSOR') +Node.RADAR = constant('NODE_RADAR') +Node.RANGE_FINDER = constant('NODE_RANGE_FINDER') +Node.RECEIVER = constant('NODE_RECEIVER') +Node.ROTATIONAL_MOTOR = constant('NODE_ROTATIONAL_MOTOR') +Node.SKIN = constant('NODE_SKIN') +Node.SPEAKER = constant('NODE_SPEAKER') +Node.TOUCH_SENSOR = constant('NODE_TOUCH_SENSOR') +Node.BALL_JOINT = constant('NODE_BALL_JOINT') +Node.BALL_JOINT_PARAMETERS = constant('NODE_BALL_JOINT_PARAMETERS') +Node.CHARGER = constant('NODE_CHARGER') +Node.CONTACT_PROPERTIES = constant('NODE_CONTACT_PROPERTIES') +Node.DAMPING = constant('NODE_DAMPING') +Node.FLUID = constant('NODE_FLUID') +Node.FOCUS = constant('NODE_FOCUS') +Node.HINGE_JOINT = constant('NODE_HINGE_JOINT') +Node.HINGE_JOINT_PARAMETERS = constant('NODE_HINGE_JOINT_PARAMETERS') +Node.HINGE_2_JOINT = constant('NODE_HINGE_2_JOINT') +Node.IMMERSION_PROPERTIES = constant('NODE_IMMERSION_PROPERTIES') +Node.JOINT_PARAMETERS = constant('NODE_JOINT_PARAMETERS') +Node.LENS = constant('NODE_LENS') +Node.LENS_FLARE = constant('NODE_LENS_FLARE') +Node.PHYSICS = constant('NODE_PHYSICS') +Node.RECOGNITION = constant('NODE_RECOGNITION') +Node.SLIDER_JOINT = constant('NODE_SLIDER_JOINT') +Node.SLOT = constant('NODE_SLOT') +Node.SOLID = constant('NODE_SOLID') +Node.SOLID_REFERENCE = constant('NODE_SOLID_REFERENCE') +Node.TRACK = constant('NODE_TRACK') +Node.TRACK_WHEEL = constant('NODE_TRACK_WHEEL') +Node.WORLD_INFO = constant('NODE_WORLD_INFO') +Node.ZOOM = constant('NODE_ZOOM') +Node.MICROPHONE = constant('NODE_MICROPHONE') +Node.RADIO = constant('NODE_RADIO') + +wb.wb_supervisor_field_get_mf_node.restype = ctypes.c_void_p +wb.wb_supervisor_field_get_sf_node.restype = ctypes.c_void_p + + +def getSFNode(self) -> Node: + return Node(ref=wb.wb_supervisor_field_get_sf_node(self._ref)) + + +def getMFNode(self, index: int) -> Node: + return Node(ref=wb.wb_supervisor_field_get_mf_node(self._ref, index)) + + +Field.getSFNode = getSFNode +Field.getMFNode = getMFNode diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/pen.py b/webots_ros2_driver/webots/lib/controller/python/controller/pen.py new file mode 100644 index 000000000..2ecd2bbe4 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/pen.py @@ -0,0 +1,29 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.device import Device +import ctypes +from typing import Union + + +class Pen(Device): + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def write(self, write: bool): + wb.wb_pen_write(self._tag, 1 if write else 0) + + def setInkColor(self, color: int, density: float): + wb.wb_pen_set_ink_color(self._tag, color, ctypes.c_double(density)) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/position_sensor.py b/webots_ros2_driver/webots/lib/controller/python/controller/position_sensor.py new file mode 100644 index 000000000..8fd133d8e --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/position_sensor.py @@ -0,0 +1,63 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.constants import constant +from controller.wb import wb +from controller.sensor import Sensor +from typing import Union + + +class PositionSensor(Sensor): + ROTATIONAL = constant('ROTATIONAL') + LINEAR = constant('LINEAR') + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_position_sensor_enable + self._get_sampling_period = wb.wb_position_sensor_get_sampling_period + super().__init__(name, sampling_period) + + wb.wb_position_sensor_get_value.restype = ctypes.c_double + + def getValue(self) -> float: + return self.value + + def getBrake(self): + return self.brake + + def getMotor(self): + return self.motor + + def getType(self) -> int: + return self.type + + @property + def brake(self): + from controller.brake import Brake + tag = wb.wb_position_sensor_get_brake(self._tag) + return None if tag == 0 else Brake(tag) + + @property + def motor(self): + from controller.motor import Motor + tag = wb.wb_brake_get_motor(self._tag) + return None if tag == 0 else Motor(tag) + + @property + def value(self) -> float: + return wb.wb_position_sensor_get_value(self._tag) + + @property + def type(self) -> int: + return wb.wb_position_sensor_get_type(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/radar.py b/webots_ros2_driver/webots/lib/controller/python/controller/radar.py new file mode 100644 index 000000000..df64a45de --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/radar.py @@ -0,0 +1,75 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.sensor import Sensor +from controller.radar_target import RadarTarget +from controller.wb import wb +from typing import List, Union + + +class Radar(Sensor): + wb.wb_radar_get_max_range.restype = ctypes.c_double + wb.wb_radar_get_min_range.restype = ctypes.c_double + wb.wb_radar_get_horizontal_fov.restype = ctypes.c_double + wb.wb_radar_get_vertical_fov.restype = ctypes.c_double + wb.wb_radar_get_targets.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_radar_enable + self._get_sampling_period = wb.wb_radar_get_sampling_period + super().__init__(name, sampling_period) + + def getMaxRange(self) -> float: + return self.max_range + + def getMinRange(self) -> float: + return self.min_range + + def getHorizontalFov(self) -> float: + return self.horizontal_fov + + def getVerticalFov(self) -> float: + return self.vertical_fov + + def getNumberOfTargets(self) -> int: + return self.number_of_targets + + def getTargets(self) -> List[RadarTarget]: + number_of_targets = self.number_of_targets + data = wb.wb_radar_get_targets(self._tag) + list = [] + for i in range(number_of_targets): + list.append(RadarTarget(data[0], data[1], data[2], data[3])) + return list + + @property + def max_range(self) -> float: + return wb.wb_radar_get_max_range(self._tag) + + @property + def min_range(self) -> float: + return wb.wb_radar_get_min_range(self._tag) + + @property + def horizontal_fov(self) -> float: + return wb.wb_radar_get_horizontal_fov(self._tag) + + @property + def vertical_fov(self) -> float: + return wb.wb_radar_get_vertical_fov(self._tag) + + @property + def number_of_targets(self) -> int: + return wb.wb_radar_get_number_of_targets(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/radar_target.py b/webots_ros2_driver/webots/lib/controller/python/controller/radar_target.py new file mode 100644 index 000000000..d19bbd6d1 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/radar_target.py @@ -0,0 +1,21 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + + +class RadarTarget: + def __init__(self, distance: float, receiver_power: float, speed: float, azimuth: float): + self.distance = distance + self.receiver_power = receiver_power + self.speed = speed + self.azimuth = azimuth diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/range_finder.py b/webots_ros2_driver/webots/lib/controller/python/controller/range_finder.py new file mode 100644 index 000000000..f6648c65f --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/range_finder.py @@ -0,0 +1,90 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +from controller.sensor import Sensor +from controller.wb import wb +from typing import List, Union + + +class RangeFinder(Sensor): + wb.wb_range_finder_get_fov.restype = ctypes.c_double + wb.wb_range_finder_get_max_range.restype = ctypes.c_double + wb.wb_range_finder_get_min_range.restype = ctypes.c_double + wb.wb_range_finder_get_range_image.restype = ctypes.POINTER(ctypes.c_float) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_range_finder_enable + self._get_sampling_period = wb.wb_range_finder_get_sampling_period + super().__init__(name, sampling_period) + + def getFov(self) -> float: + return self.fov + + def getWidth(self) -> int: + return self.width + + def getHeight(self) -> int: + return self.height + + def getMaxRange(self) -> float: + return self.max_range + + def getMinRange(self) -> float: + return self.min_range + + def getRangeImage(self, data_type='list') -> List[float]: + if data_type == 'list': + return self.range_image[:self.width * self.height] + else: + return self.range_image + + def getRangeImageArray(self) -> List[List[float]]: + range_image = self.range_image + width = self.width + array = [] + for i in range(self.height): + array.append(range_image[i * width:(i + 1) * width]) + return array + + @staticmethod + def rangeImageGetDepth(image, width, x, y) -> float: + return image[y * width + x] + + def saveImage(self, filename: str, quality: int) -> int: + return wb.wb_range_finder_save_image(self._tag, str.encode(filename), quality) + + @property + def range_image(self): + return wb.wb_range_finder_get_range_image(self._tag) + + @property + def fov(self) -> float: + return wb.wb_range_finder_get_fov(self._tag) + + @property + def width(self) -> int: + return wb.wb_range_finder_get_width(self._tag) + + @property + def height(self) -> int: + return wb.wb_range_finder_get_height(self._tag) + + @property + def max_range(self) -> float: + return wb.wb_range_finder_get_max_range(self._tag) + + @property + def min_range(self) -> float: + return wb.wb_range_finder_get_min_range(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/receiver.py b/webots_ros2_driver/webots/lib/controller/python/controller/receiver.py new file mode 100644 index 000000000..8e68f42af --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/receiver.py @@ -0,0 +1,103 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +import struct +import sys +from controller.wb import wb +from controller.sensor import Sensor +from typing import Union, Tuple, List + + +class Receiver(Sensor): + wb.wb_receiver_get_data.restype = ctypes.POINTER(ctypes.c_ubyte) + wb.wb_receiver_get_signal_strength.restype = ctypes.c_double + wb.wb_receiver_get_emitter_direction.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: Union[str, int], sampling_period: int = None): + self._enable = wb.wb_receiver_enable + self._get_sampling_period = wb.wb_receiver_get_sampling_period + super().__init__(name, sampling_period) + + def getBytes(self) -> bytes: + return bytes(self.data[0:self.data_size]) + + def getData(self) -> str: + print('DEPRECATION: Receiver.getData is deprecated, please use Receiver.getString instead.', file=sys.stderr) + return self.string + + def getDataSize(self) -> int: + return self.data_size + + def getFloats(self) -> Tuple[float, ...]: + return struct.unpack(f'{int(self.data_size / 8)}d', self.getBytes()) + + def getInts(self) -> Tuple[int, ...]: + return struct.unpack(f'{int(self.data_size / 4)}i', self.getBytes()) + + def getBools(self) -> Tuple[bool, ...]: + return struct.unpack(f'{self.data_size}?', self.getBytes()) + + def getQueueLength(self) -> int: + return self.queue_length + + def getString(self) -> str: + return self.string + + def nextPacket(self): + wb.wb_receiver_next_packet(self._tag) + + def getSignalStrength(self): + return self.signal_strength + + def getEmitterDirection(self) -> List[float]: + return self.emitter_direction + + def getChannel(self) -> int: + return self.channel + + def setChannel(self, channel: int): + self.channel = channel + + @property + def queue_length(self) -> int: + return wb.wb_receiver_get_queue_length(self._tag) + + @property + def data(self) -> bytes: + return wb.wb_receiver_get_data(self._tag) + + @property + def data_size(self) -> int: + return wb.wb_receiver_get_data_size(self._tag) + + @property + def string(self) -> str: + return ctypes.string_at(self.data, self.data_size).decode() + + @property + def signal_strength(self) -> float: + return wb.wb_receiver_get_signal_strength(self._tag) + + @property + def emitter_direction(self): + return wb.wb_receiver_get_emitter_direction(self._tag) + + @property + def channel(self) -> int: + return wb.wb_receiver_get_channel(self._tag) + + @channel.setter + def channel(self, channel: int): + wb.wb_receiver_set_channel(self._tag, channel) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/robot.py b/webots_ros2_driver/webots/lib/controller/python/controller/robot.py new file mode 100644 index 000000000..83968e910 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/robot.py @@ -0,0 +1,395 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +import sys +import typing +from controller.wb import wb +from controller.node import Node +from controller.device import Device +from controller.accelerometer import Accelerometer +from controller.altimeter import Altimeter +from controller.brake import Brake +from controller.camera import Camera +from controller.compass import Compass +from controller.connector import Connector +from controller.display import Display +from controller.distance_sensor import DistanceSensor +from controller.emitter import Emitter +from controller.gps import GPS +from controller.gyro import Gyro +from controller.inertial_unit import InertialUnit +from controller.led import LED +from controller.lidar import Lidar +from controller.light_sensor import LightSensor +from controller.motor import Motor +from controller.pen import Pen +from controller.position_sensor import PositionSensor +from controller.radar import Radar +from controller.range_finder import RangeFinder +from controller.receiver import Receiver +from controller.skin import Skin +from controller.speaker import Speaker +from controller.touch_sensor import TouchSensor + +from controller.joystick import Joystick +from controller.keyboard import Keyboard +from controller.mouse import Mouse + + +class Robot: + 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 + MODE_SIMULATION = 0 + MODE_CROSS_COMPILATION = 1 + MODE_REMOTE_CONTROL = 2 + created = None + wb.wb_robot_get_name.restype = ctypes.c_char_p + wb.wb_robot_get_model.restype = ctypes.c_char_p + wb.wb_robot_get_custom_data.restype = ctypes.c_char_p + wb.wb_robot_get_basic_time_step.restype = ctypes.c_double + wb.wb_robot_get_time.restype = ctypes.c_double + wb.wb_robot_get_name.restype = ctypes.c_char_p + wb.wb_robot_battery_sensor_get_value.restype = ctypes.c_double + wb.wb_robot_wwi_receive_text.restype = ctypes.c_char_p + wb.wb_robot_get_urdf.restype = ctypes.c_char_p + wb.wb_robot_get_project_path.restype = ctypes.c_char_p + wb.wb_robot_get_world_path.restype = ctypes.c_char_p + + def __init__(self): + if Robot.created: + print('Error: only one Robot instance can be created per controller process.', file=sys.stderr) + return + Robot.created = self + wb.wb_robot_init() + self.devices = {} + n = wb.wb_robot_get_number_of_devices() + for i in range(0, n): + tag = wb.wb_robot_get_device_by_index(i) + name = wb.wb_device_get_name(tag).decode() + type = wb.wb_device_get_node_type(tag) + if type == Node.ACCELEROMETER: + self.devices[name] = Accelerometer(tag) + elif type == Node.ALTIMETER: + self.devices[name] = Altimeter(name) + elif type == Node.BRAKE: + self.devices[name] = Brake(tag) + elif type == Node.CAMERA: + self.devices[name] = Camera(tag) + elif type == Node.COMPASS: + self.devices[name] = Compass(tag) + elif type == Node.CONNECTOR: + self.devices[name] = Connector(tag) + elif type == Node.DISPLAY: + self.devices[name] = Display(tag) + elif type == Node.DISTANCE_SENSOR: + self.devices[name] = DistanceSensor(tag) + elif type == Node.EMITTER: + self.devices[name] = Emitter(tag) + elif type == Node.GPS: + self.devices[name] = GPS(tag) + elif type == Node.GYRO: + self.devices[name] = Gyro(tag) + elif type == Node.INERTIAL_UNIT: + self.devices[name] = InertialUnit(tag) + elif type == Node.LED: + self.devices[name] = LED(tag) + elif type == Node.LIDAR: + self.devices[name] = Lidar(tag) + elif type == Node.LIGHT_SENSOR: + self.devices[name] = LightSensor(tag) + elif type == Node.LINEAR_MOTOR or type == Node.ROTATIONAL_MOTOR: + self.devices[name] = Motor(tag) + elif type == Node.PEN: + self.devices[name] = Pen(tag) + elif type == Node.POSITION_SENSOR: + self.devices[name] = PositionSensor(tag) + elif type == Node.RADAR: + self.devices[name] = Radar(tag) + elif type == Node.RANGE_FINDER: + self.devices[name] = RangeFinder(tag) + elif type == Node.RECEIVER: + self.devices[name] = Receiver(tag) + elif type == Node.SKIN: + self.devices[name] = Skin(tag) + elif type == Node.SPEAKER: + self.devices[name] = Speaker(tag) + elif type == Node.TOUCH_SENSOR: + self.devices[name] = TouchSensor(tag) + else: + print('Unsupported device type: ' + str(type) + ' for device named "' + name + '"', file=sys.stderr) + self.keyboard = Keyboard(0) + self.mouse = Mouse(0) + self.joystick = Joystick(0) + + def __del__(self): + wb.wb_robot_cleanup() + + def getAccelerometer(self, name: str) -> Accelerometer: + print('DEPRECATION: Robot.getAccelerometer is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getAltimeter(self, name: str) -> Altimeter: + print('DEPRECATION: Robot.getAltimeter is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getBrake(self, name: str) -> Brake: + print('DEPRECATION: Robot.getBrake is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getCamera(self, name: str) -> Camera: + print('DEPRECATION: Robot.getCamera is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getCompass(self, name: str) -> Compass: + print('DEPRECATION: Robot.getCompass is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getConnector(self, name: str) -> Connector: + print('DEPRECATION: Robot.getConnector is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getDisplay(self, name: str) -> Display: + print('DEPRECATION: Robot.getDisplay is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getDistanceSensor(self, name: str) -> DistanceSensor: + print('DEPRECATION: Robot.getDistanceSensor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getEmitter(self, name: str) -> Emitter: + print('DEPRECATION: Robot.getEmitter is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getGPS(self, name: str) -> GPS: + print('DEPRECATION: Robot.getGPS is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getGyro(self, name: str) -> Gyro: + print('DEPRECATION: Robot.getGyro is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getInertialUnit(self, name: str) -> InertialUnit: + print('DEPRECATION: Robot.getInertialUnit is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getLED(self, name: str) -> LED: + print('DEPRECATION: Robot.getLed is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getLidar(self, name: str) -> Lidar: + print('DEPRECATION: Robot.getLidar is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getLightSensor(self, name: str) -> LightSensor: + print('DEPRECATION: Robot.getLightSensor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getMotor(self, name: str) -> Motor: + print('DEPRECATION: Robot.getMotor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getPen(self, name: str) -> Pen: + print('DEPRECATION: Robot.getPen is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getPositionSensor(self, name: str) -> PositionSensor: + print('DEPRECATION: Robot.getPositionSensor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getRadar(self, name: str) -> Radar: + print('DEPRECATION: Robot.getRadar is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getRangeFinder(self, name: str) -> RangeFinder: + print('DEPRECATION: Robot.getRangeFinder is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getReceiver(self, name: str) -> Receiver: + print('DEPRECATION: Robot.getReceiver is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getSkin(self, name: str) -> Skin: + print('DEPRECATION: Robot.getSkin is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getSpeaker(self, name: str) -> Speaker: + print('DEPRECATION: Robot.getSpeaker is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getTouchSensor(self, name: str) -> TouchSensor: + print('DEPRECATION: Robot.getTouchSensor is deprecated, please use Robot.getDevice instead.', file=sys.stderr) + return self.getDevice(name) + + def getDevice(self, name: str) -> Device: + if name not in self.devices: + print(f'Device "{name}" was not found on robot "{self.name}"', file=sys.stderr) + return None + else: + return self.devices[name] + + def getKeyboard(self) -> Keyboard: + return self.keyboard + + def getMouse(self) -> Mouse: + return self.mouse + + def getJoystick(self) -> Joystick: + return self.joystick + + def getDeviceByIndex(self, index: int) -> Device: + tag = wb.wb_robot_get_device_by_index(index) + name = wb.wb_device_get_name(tag).decode() + return self.devices[name] + + def getBasicTimeStep(self) -> float: + return self.basic_time_step + + def getName(self) -> str: + return self.name + + def getModel(self) -> str: + return self.model + + def getCustomData(self) -> str: + return self.custom_data + + def setCustomData(self, data: str): + self.custom_data = data + + def getProjectPath(self) -> str: + return self.project_path + + def getWorldPath(self) -> str: + return self.world_path + + def getSupervisor(self) -> bool: + return self.supervisor + + def getSynchronization(self) -> bool: + return self.synchronization + + def getNumberOfDevices(self) -> int: + return self.number_of_devices + + def getTime(self) -> float: + return self.time + + def getUrdf(self, prefix: str = '') -> str: + return wb.wb_robot_get_urdf(str.encode(prefix)).decode() + + def wwiSendText(self, text: str): + wb.wb_robot_wwi_send(str.encode(text), len(text) + 1) + + def wwiReceiveText(self) -> typing.Union[str, None]: + text = wb.wb_robot_wwi_receive_text() + return None if text is None else text.decode() + + def step(self, time_step: int = None) -> int: + if time_step is None: + time_step = int(self.basic_time_step) + return wb.wb_robot_step(time_step) + + def stepBegin(self, time_step: int = None) -> int: + if time_step is None: + time_step = int(self.basic_time_step) + return wb.wb_robot_step_begin(time_step) + + def stepEnd(self) -> int: + return wb.wb_robot_step_end() + + def waitForUserInputEvent(self, event_type: int, timeout: int) -> int: + return wb.wb_robot_wait_for_user_input_event(event_type, timeout) + + def batterySensorEnable(self, sampling_period: int): + wb.wb_robot_battery_sensor_enable(sampling_period) + + def batterySensorDisable(self): + wb.wb_robot_battery_sensor_disable() + + def batterySensorGetSamplingPeriod(self) -> int: + return self.battery_sensor_sampling_period + + def batterySensorGetValue(self) -> float: + return wb.wb_robot_battery_sensor_get_value() + + def getMode(self) -> int: + return self.mode + + def setMode(self, mode: int, arg: str): + wb.wb_robot_set_mode(mode, str.encode(arg)) + + @property + def basic_time_step(self) -> float: + return wb.wb_robot_get_basic_time_step() + + @property + def name(self) -> str: + return wb.wb_robot_get_name().decode() + + @property + def model(self) -> str: + return wb.wb_robot_get_model().decode() + + @property + def custom_data(self) -> str: + return wb.wb_robot_get_custom_data().decode() + + @custom_data.setter + def custom_data(self, data: str): + wb.wb_robot_set_custom_data(str.encode(data)) + + @property + def project_path(self) -> str: + return wb.wb_robot_get_project_path().decode() + + @property + def world_path(self) -> str: + return wb.wb_robot_get_world_path().decode() + + @property + def supervisor(self) -> bool: + return wb.wb_robot_get_supervisor() != 0 + + @property + def synchronization(self) -> bool: + return wb.wb_robot_get_synchronization() != 0 + + @property + def time(self) -> float: + return wb.wb_robot_get_time() + + @property + def number_of_devices(self) -> int: + return wb.wb_robot_get_number_of_devices() + + @property + def battery_sensor_sampling_period(self) -> int: + return wb.wb_robot_battery_sensor_get_sampling_period() + + @battery_sensor_sampling_period.setter + def battery_sensor_sampling_period(self, sampling_period: int): + if sampling_period is None: + sampling_period = int(self.basic_time_step) + wb.wb_robot_battery_sensor_enable(sampling_period) + + @property + def mode(self) -> int: + return wb.wb_robot_get_mode() diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/sensor.py b/webots_ros2_driver/webots/lib/controller/python/controller/sensor.py new file mode 100644 index 000000000..bd765b511 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/sensor.py @@ -0,0 +1,43 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.device import Device +from typing import Union + + +class Sensor(Device): + def __init__(self, name: Union[str, int], sampling_period: int = None): + super().__init__(name) + if isinstance(name, str): + self.sampling_period = int(wb.wb_robot_get_basic_time_step()) if sampling_period is None else sampling_period + + def enable(self, p: int): + self._enable(self._tag, p) + + def getSamplingPeriod(self) -> int: + return self.sampling_period + + def disable(self): + self._enable(self._tag, 0) + + @property + def sampling_period(self) -> int: + return self._get_sampling_period(self._tag) + + @sampling_period.setter + def sampling_period(self, p: Union[int, None]): + if p is None: + p = 0 + self._enable(self._tag, p) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/skin.py b/webots_ros2_driver/webots/lib/controller/python/controller/skin.py new file mode 100644 index 000000000..bf2326088 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/skin.py @@ -0,0 +1,53 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.device import Device +from controller.wb import wb +import ctypes +from typing import List, Union + + +class Skin(Device): + wb.wb_skin_get_bone_name.restype = ctypes.c_char_p + wb.wb_skin_get_bone_orientation.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_skin_get_bone_position.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_skin_set_bone_orientation.argtypes = [ctypes.c_int, ctypes.c_int, ctypes.POINTER(ctypes.c_double), ctypes.c_char] + wb.wb_skin_set_bone_position.argtypes = [ctypes.c_int, ctypes.c_int, ctypes.POINTER(ctypes.c_double), ctypes.c_char] + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def getBoneCount(self) -> int: + return self.bone_count + + def getBoneName(self, index: int) -> str: + return wb.wb_skin_get_bone_name(self._tag, index).decode() + + def getBoneOrientation(self, index: int, absolute: bool) -> List[float]: + return wb.wb_skin_get_bone_orientation(self._tag, index, 1 if absolute else 0) + + def getBonePosition(self, index: int, absolute: bool) -> List[float]: + return wb.wb_skin_get_bone_position(self._tag, index, 1 if absolute else 0) + + def setBoneOrientation(self, index: int, orientation: List[float], absolute: bool): + data = (ctypes.c_double * 4)(orientation[0], orientation[1], orientation[2], orientation[3]) + wb.wb_skin_set_bone_orientation(self._tag, index, data, 1 if absolute else 0) + + def setBonePosition(self, index: int, position: List[float], absolute: bool): + data = (ctypes.c_double * 3)(position[0], position[1], position[2]) + wb.wb_skin_set_bone_position(self._tag, index, data, 1 if absolute else 0) + + @property + def bone_count(self) -> int: + return wb.wb_skin_get_bone_count(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/speaker.py b/webots_ros2_driver/webots/lib/controller/python/controller/speaker.py new file mode 100644 index 000000000..f2f75a6ed --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/speaker.py @@ -0,0 +1,59 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.device import Device +import ctypes +from typing import Union + + +class Speaker(Device): + wb.wb_speaker_get_engine.restype = ctypes.c_char_p + wb.wb_speaker_get_language.restype = ctypes.c_char_p + + def __init__(self, name: Union[str, int]): + super().__init__(name) + + def stop(self, sound: Union[str, None] = None): + wb.wb_speaker_stop(self._tag, None if sound is None else str.encode(sound)) + + def isSoundPlaying(self, sound: str) -> bool: + return wb.wb_speaker_is_sound_playing(self._tag, str.encode(sound)) != 0 + + def setEngine(self, engine: str) -> bool: + return wb.wb_speaker_set_engine(self._tag, str.encode(engine)) != 0 + + def setLanguage(self, language: str) -> bool: + return wb.wb_speaker_set_engine(self._tag, str.encode(language)) != 0 + + def getLanguage(self) -> str: + return wb.wb_speaker_get_language(self._tag).decode() + + def getEngine(self) -> str: + return wb.wb_speaker_get_engine(self._tag).decode() + + def isSpeaking(self) -> bool: + return wb.wb_speaker_is_speaking(self._tag) != 0 + + def speak(self, text: str, volume: float): + wb.wb_speaker_speak(self._tag, str.encode(text), ctypes.c_double(volume)) + + +def _playSound(left: Speaker, right: Speaker, sound: str, volume: float, pitch: float, balance: float, loop: bool): + wb.wb_speaker_play_sound(left._tag, right._tag, str.encode(sound), + ctypes.c_double(volume), ctypes.c_double(pitch), ctypes.c_double(balance), + 1 if loop else 0) + + +Speaker.playSound = staticmethod(_playSound) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/supervisor.py b/webots_ros2_driver/webots/lib/controller/python/controller/supervisor.py new file mode 100644 index 000000000..929ad0a53 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/supervisor.py @@ -0,0 +1,109 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.wb import wb +from controller.node import Node +from controller.robot import Robot +import ctypes + + +class Supervisor(Robot): + def __init__(self): + super().__init__() + + def getRoot(self) -> Node: + return Node() + + def getSelf(self) -> Node: + return Node(tag=0) + + def getFromDef(self, d: str) -> Node: + node = Node(DEF=d) + return node if node._ref else None + + def getFromId(self, id) -> Node: + node = Node(id=id) + return node if node._ref else None + + def getFromDevice(self, tag) -> Node: + node = Node(tag=tag) + return node if node._ref else None + + def getSelected(self) -> Node: + node = Node(selected=True) + return node if node._ref else None + + def setLabel(self, id, label, x, y, size, color, transparency=0, font='Arial'): + wb.wb_supervisor_set_label(id, str.encode(label), ctypes.c_double(x), ctypes.c_double(y), ctypes.c_double(size), + color, ctypes.c_double(transparency), str.encode(font)) + + def simulationQuit(self, status: int): + wb.wb_supervisor_simulation_quit(status) + + def simulationSetMode(self, mode: int): + self.simulation_mode = mode + + def simulationGetMode(self) -> int: + return self.simulation_mode + + def simulationReset(self): + wb.wb_supervisor_simulation_reset() + + def simulationResetPhysics(self): + wb.wb_supervisor_simulation_reset_physics() + + def worldLoad(self, filename: str): + wb.wb_supervisor_world_load(str.encode(filename)) + + def worldSave(self, filename: str) -> int: + return wb.wb_supervisor_world_save(str.encode(filename)) + + def worldReload(self): + wb.wb_supervisor_world_reload() + + def exportImage(self, filename: str, quality: int): + wb.wb_supervisor_export_image(str.encode(filename), quality) + + def movieStartRecording(self, filename: str, width: int, height: int, codec: int, quality: int, acceleration: int, + caption: bool): + wb.wb_supervisor_movie_start_recording(str.encode(filename), width, height, codec, quality, acceleration, + 1 if caption else 0) + + def movieStopRecording(self): + wb.wb_supervisor_movie_stop_recording() + + def movieIsReady(self): + return wb.wb_supervisor_movie_is_ready() != 0 + + def movieFailed(self): + return wb.wb_supervisor_movie_failed() != 0 + + def animationStartRecording(self, filename: str): + return wb.wb_supervisor_animation_start_recording(str.encode(filename)) + + def animationStopRecording(self): + return wb.wb_supervisor_animation_stop_recording() + + @property + def simulation_mode(self) -> int: + return wb.wb_supervisor_simulation_get_mode() + + @simulation_mode.setter + def mode(self, mode: int): + return wb.wb_supervisor_simulation_set_mode(mode) + + +Supervisor.SIMULATION_MODE_PAUSE = 0 +Supervisor.SIMULATION_MODE_REAL_TIME = 1 +Supervisor.SIMULATION_MODE_FAST = 2 diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/touch_sensor.py b/webots_ros2_driver/webots/lib/controller/python/controller/touch_sensor.py new file mode 100644 index 000000000..b0bedb747 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/touch_sensor.py @@ -0,0 +1,61 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller.sensor import Sensor +from controller.wb import wb +import ctypes +import typing + + +class TouchSensor(Sensor): + BUMPER = 0 + FORCE = 1 + FORCE3D = 2 + wb.wb_touch_sensor_get_value.restype = ctypes.c_double + wb.wb_touch_sensor_get_values.restype = ctypes.POINTER(ctypes.c_double) + wb.wb_touch_sensor_get_lookup_table.restype = ctypes.POINTER(ctypes.c_double) + + def __init__(self, name: typing.Union[str, int], sampling_period: int = None): + self._enable = wb.wb_touch_sensor_enable + self._get_sampling_period = wb.wb_touch_sensor_get_sampling_period + super().__init__(name, sampling_period) + + def getValue(self) -> float: + return self.value + + def getValues(self) -> typing.List[float]: + return self.values + + def getLookupTable(self) -> typing.List[float]: + return self.lookup_table + + def getType(self) -> int: + return self.type + + @property + def value(self) -> float: + return wb.wb_touch_sensor_get_value(self._tag) + + @property + def values(self): + return wb.wb_touch_sensor_get_values(self._tag) + + @property + def lookup_table(self) -> typing.List[float]: + size = wb.wb_touch_sensor_get_lookup_table_size(self._tag) + return wb.wb_touch_sensor_get_lookup_table(self._tag)[: 3 * size] + + @property + def type(self) -> int: + return wb.wb_touch_sensor_get_type(self._tag) diff --git a/webots_ros2_driver/webots/lib/controller/python/controller/wb.py b/webots_ros2_driver/webots/lib/controller/python/controller/wb.py new file mode 100644 index 000000000..12845e647 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/controller/wb.py @@ -0,0 +1,26 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import ctypes +import os +import sys + +if sys.platform == 'linux' or sys.platform == 'linux2': + path = os.path.join('lib', 'controller', 'libController.so') +elif sys.platform == 'win32': + path = os.path.join('lib', 'controller', 'Controller.dll') +elif sys.platform == 'darwin': + path = os.path.join('Contents', 'MacOS', 'lib', 'controller', 'libController.dylib') + +wb = ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path)) diff --git a/webots_ros2_driver/webots/lib/controller/python/vehicle/__init__.py b/webots_ros2_driver/webots/lib/controller/python/vehicle/__init__.py new file mode 100644 index 000000000..7e4e6432f --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/vehicle/__init__.py @@ -0,0 +1,16 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from vehicle.driver import Driver # noqa +from vehicle.car import Car # noqa diff --git a/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py b/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py new file mode 100644 index 000000000..05111dd00 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/vehicle/car.py @@ -0,0 +1,178 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller import Supervisor +import ctypes +import os +import sys + + +class Car(Supervisor): + # types + TRACTION = 0 + PROPULSION = 1 + FOUR_BY_FOUR = 2 + # engine types + COMBUSTION_ENGINE = 0 + ELECTRIC_ENGINE = 1 + PARALLEL_HYBRID_ENGINE = 2 + POWER_SPLIT_HYBRID_ENGINE = 3 + # wheels + WHEEL_FRONT_RIGHT = 0 + WHEEL_FRONT_LEFT = 1 + WHEEL_REAR_RIGHT = 2 + WHEEL_REAR_LEFT = 3 + + def __init__(self): + super().__init__() + if sys.platform == 'linux' or sys.platform == 'linux2': + path = os.path.join('lib', 'controller', 'libcar.so') + elif sys.platform == 'win32': + path = os.path.join('lib', 'controller', 'car.dll') + elif sys.platform == 'darwin': + path = os.path.join('Contents', 'MacOS', 'lib', 'controller', 'libcar.dylib') + self.api = ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path)) + self.api.wbu_car_get_front_wheel_radius.restype = ctypes.c_double + self.api.wbu_car_get_indicator_period.restype = ctypes.c_double + self.api.wbu_car_get_left_steering_angle.restype = ctypes.c_double + self.api.wbu_car_get_rear_wheel_radius.restype = ctypes.c_double + self.api.wbu_car_get_right_steering_angle.restype = ctypes.c_double + self.api.wbu_car_get_track_front.restype = ctypes.c_double + self.api.wbu_car_get_track_rear.restype = ctypes.c_double + self.api.wbu_car_get_wheel_base.restype = ctypes.c_double + self.api.wbu_car_get_wheel_encoder.restype = ctypes.c_double + self.api.wbu_car_get_wheel_speed.restype = ctypes.c_double + + self.api.wbu_car_init() + + def __del__(self): + self.api.wbu_can_cleanup() + super().__del__() + + def enableIndicatorAutoDisabling(self, enable: bool): + self.api.wbu_car_enable_indicator_auto_disabling(1 if enable else 0) + + def enableLimitedSlipDifferential(self, enable: bool): + self.api.wbu_car_enable_limited_slip_differential(1 if enable else 0) + + def getBackwardsLights(self) -> bool: + return self.backwards_lights + + def getBrakeLights(self) -> bool: + return self.brake_lights + + def getEngineType(self) -> int: + return self.engine_type + + def getFrontWheelRadius(self): + return self.front_wheel_radius + + def getIndicatorPeriod(self) -> float: + return self.indicator_period + + def getLeftSteeringAngle(self): + return self.left_steering_angle + + def getRearWheelRadius(self): + return self.rear_wheel_radius + + def getRightSteeringAngle(self): + return self.right_steering_angle + + def getTrackFront(self): + return self.track_front + + def getTrackRear(self): + return self.track_rear + + def getType(self) -> int: + return self.type + + def getWheelbase(self): + return self.wheel_base + + def getWheelEncoder(self, wheel_index): + return self.api.wbu_car_get_wheel_encoder(wheel_index) + + def getWheelSpeed(self, wheel_index): + return self.api.wbu_car_get_wheel_speed(wheel_index) + + def setIndicatorPeriod(self, period: float): + self.indicator_period = period + + def setLeftSteeringAngle(self, angle): + self.left_steering_angle = angle + + def setRightSteeringAngle(self, angle): + self.right_steering_angle = angle + + @property + def backwards_lights(self) -> bool: + return self.api.wbu_car_get_backwards_lights() != 0 + + @property + def brake_lights(self) -> bool: + return self.api.wbu_car_get_brake_lights() != 0 + + @property + def engine_type(self) -> int: + return self.api.wbu_car_get_engine_type() + + @property + def front_wheel_radius(self) -> float: + return self.api.wbu_car_get_front_wheel_radius() + + @property + def indicator_period(self) -> float: + return self.api.wbu_car_get_indicator_period() + + @indicator_period.setter + def indicator_period(self, period: float): + self.api.wbu_car_set_indicator_period(ctypes.c_double(period)) + + @property + def left_steering_angle(self) -> float: + return self.api.wbu_car_get_left_steering_angle() + + @left_steering_angle.setter + def left_steering_angle(self, angle: float): + self.api.wbu_car_set_left_steering_angle(ctypes.c_double(angle)) + + @property + def rear_wheel_radius(self) -> float: + return self.api.wbu_car_get_rear_wheel_radius() + + @property + def right_steering_angle(self) -> float: + return self.api.wbu_car_get_right_steering_angle() + + @right_steering_angle.setter + def right_steering_angle(self, angle: float): + self.api.wbu_car_set_right_steering_angle(ctypes.c_double(angle)) + + @property + def track_front(self) -> float: + return self.api.wbu_car_get_track_front() + + @property + def track_rear(self) -> float: + return self.api.wbu_car_get_track_rear() + + @property + def type(self) -> int: + return self.api.wbu_car_get_type() + + @property + def wheel_base(self) -> float: + return self.api.wbu_car_get_wheel_base() diff --git a/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py b/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py new file mode 100644 index 000000000..e4214af17 --- /dev/null +++ b/webots_ros2_driver/webots/lib/controller/python/vehicle/driver.py @@ -0,0 +1,246 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from controller import Supervisor +import ctypes +import os +import sys + + +class Driver(Supervisor): + INDICATOR_OFF = 0 + INDICATOR_RIGHT = 1 + INDICATOR_LEFT = 2 + SPEED = 0 + TORQUE = 1 + # wiper modes + DOWN = 0 + SLOW = 1 + NORMAL = 2 + FAST = 3 + + api = None + + @staticmethod + def loadApi() -> ctypes.cdll: + if sys.platform == 'linux' or sys.platform == 'linux2': + path = os.path.join('lib', 'controller') + car = 'libcar.so' + driver = 'libdriver.so' + elif sys.platform == 'win32': + path = os.path.join('lib', 'controller') + car = 'car.dll' + driver = 'driver.dll' + elif sys.platform == 'darwin': + path = os.path.join('Contents', 'MacOS', 'lib', 'controller') + car = 'libcar.dylib' + driver = 'libdriver.dylib' + ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path, car)) + Driver.api = ctypes.cdll.LoadLibrary(os.path.join(os.environ['WEBOTS_HOME'], path, driver)) + + def __init__(self): + super().__init__() + if not Driver.api: + Driver.loadApi() + Driver.api.wbu_driver_get_brake_intensity.restype = ctypes.c_double + Driver.api.wbu_driver_get_current_speed.restype = ctypes.c_double + Driver.api.wbu_driver_get_rpm.restype = ctypes.c_double + Driver.api.wbu_driver_get_steering_angle.restype = ctypes.c_double + Driver.api.wbu_driver_get_target_cruising_speed.restype = ctypes.c_double + Driver.api.wbu_driver_get_throttle.restype = ctypes.c_double + Driver.api.wbu_driver_init() + + def __del__(self): + Driver.api.wbu_driver_cleanup() + super().__del__() + + def getAntifogLights(self) -> bool: + return self.antifog_lights + + def getBrakeIntensity(self) -> float: + return self.brake_intensity + + def getControlMode(self) -> int: + return self.control_mode + + def getCurrentSpeed(self) -> float: + return self.current_speed + + def getDippedBeams(self) -> bool: + return self.dipped_beams + + def getGear(self): + return self.gear + + def getGearNumber(self): + return self.gear_number + + def getHazardFlashers(self) -> bool: + return self.hazard_flashers + + def getIndicator(self) -> int: + return self.indicator + + def getRpm(self) -> float: + return self.rpm + + def getSteeringAngle(self) -> float: + return self.steering_angle + + def getTargetCruisingSpeed(self) -> float: + return self.target_cruising_speed + + def getThrottle(self) -> float: + return self.throttle + + def getWiperMode(self) -> int: + return Driver.api.wbu_driver_get_wiper_mode() + + def setAntifogLights(self, state: bool): + return self.antifog_lights + + def setBrakeIntensity(self, brakeIntensity: float): + self.brake_intensity = brakeIntensity + + def setCruisingSpeed(self, cruisingSpeed: float): + self.target_cruising_speed = cruisingSpeed + + def setDippedBeams(self, state: bool): + self.dipped_beams = state + + def setGear(self, gear): + self.gear = gear + + def setHazardFlashers(self, hazardFlasher: bool): + self.hazard_flashers = hazardFlasher + + def setIndicator(self, indicator: int): + self.indicator = indicator + + def setSteeringAngle(self, steeringAngle: float): + self.steering_angle = steeringAngle + + def setThrottle(self, throttle: float): + self.throttle = throttle + + def setWiperMode(self, mode: int): + self.wiper_mode = mode + + def step(self): + return Driver.api.wbu_driver_step() + + @property + def antifog_lights(self) -> bool: + return Driver.api.wbu_driver_get_antifog_lights() != 0 + + @antifog_lights.setter + def antifog_lights(self, antifog_lights: bool): + Driver.api.wbu_driver_set_antifog_lights(1 if antifog_lights else 0) + + @property + def brake_intensity(self) -> float: + return Driver.api.wbu_driver_get_brake_intensity() + + @brake_intensity.setter + def brake_intensity(self, brake_intensity: float): + Driver.api.wbu_driver_set_brake_intensity(ctypes.c_double(brake_intensity)) + + @property + def control_mode(self) -> int: + m = Driver.api.wbu_driver_get_control_mode() + return None if m == -1 else m + + @property + def dipped_beams(self) -> bool: + return Driver.api.wbu_driver_get_dipped_beams() != 0 + + @dipped_beams.setter + def dipped_beams(self, dipped_beams: bool): + Driver.api.wbu_driver_set_dipped_beams(1 if dipped_beams else 0) + + @property + def gear(self) -> int: + return Driver.api.wbu_driver_get_gear() + + @gear.setter + def gear(self, gear: int): + Driver.api.wbu_driver_set_gear(gear) + + @property + def gear_number(self) -> int: + return Driver.api.wbu_driver_get_gear_number() + + @property + def hazard_flashers(self) -> bool: + return Driver.api.wbu_driver_get_hazard_flashers() != 0 + + @hazard_flashers.setter + def hazard_flashers(self, hazard_flashers: bool): + Driver.api.wbu_driver_set_hazard_flashers(1 if hazard_flashers else 0) + + @property + def indicator(self) -> int: + return Driver.api.wbu_driver_get_indicator() + + @indicator.setter + def indicator(self, indicator: int): + Driver.api.wb_driver_set_indicator(indicator) + + @property + def rpm(self) -> float: + return Driver.api.wbu_driver_get_rpm() + + @property + def steering_angle(self) -> float: + return Driver.api.wbu_driver_get_steering_angle() + + @steering_angle.setter + def steering_angle(self, value: float): + Driver.api.wbu_driver_set_steering_angle(ctypes.c_double(value)) + + @property + def current_speed(self) -> float: + return Driver.api.wbu_driver_get_current_speed() + + @property + def target_cruising_speed(self) -> float: + return Driver.api.wbu_driver_get_target_cruising_speed() + + @target_cruising_speed.setter + def target_cruising_speed(self, target_cruising_speed: float): + Driver.api.wbu_driver_set_cruising_speed(ctypes.c_double(target_cruising_speed)) + + @property + def throttle(self) -> float: + return Driver.api.wbu_driver_get_throttle() + + @throttle.setter + def throttle(self, throttle: float): + Driver.api.wbu_driver_set_throttle(ctypes.c_double(throttle)) + + @property + def wiper_mode(self) -> int: + return Driver.api.wbu_driver_get_wiper_mode() + + @wiper_mode.setter + def wiper_mode(self, mode): + Driver.api.wbu_driver_set_wiper_mode(mode) + + # private function for webots_ros2 to identify robots that can use libdriver + @staticmethod + def isInitialisationPossible() -> bool: + if not Driver.api: + Driver.loadApi() + Driver.api.wbu_driver_initialization_is_possible.restype = ctypes.c_bool + return Driver.api.wbu_driver_initialization_is_possible() diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/Makefile new file mode 100644 index 000000000..8ce942220 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/Makefile @@ -0,0 +1,42 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +.PHONY: release debug profile clean c/car c/driver cpp/car cpp/driver python java + +release debug profile clean: java python + +java: cpp/driver cpp/car + +@echo "# make" $(MAKECMDGOALS) $@ + +@make -s -C $@ $(MAKECMDGOALS) + +cpp/car: cpp/driver c/car + +@echo "# make" $(MAKECMDGOALS) $@ + +@make -s -C $@ $(MAKECMDGOALS) + +cpp/driver: c/driver + +@echo "# make" $(MAKECMDGOALS) $@ + +@make -s -C $@ $(MAKECMDGOALS) + +c/driver: c/car + +@echo "# make" $(MAKECMDGOALS) $@ + +@make -s -C $@ $(MAKECMDGOALS) + +c/car: + +@echo "# make" $(MAKECMDGOALS) $@ + +@make -s -C c/car $(MAKECMDGOALS) diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/Makefile new file mode 100644 index 000000000..963a93c27 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/Makefile @@ -0,0 +1,39 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +WEBOTS_LIBRARY = 1 +C_SOURCES = $(wildcard src/*.c) + +LIBRARIES = -L$(WEBOTS_CONTROLLER_LIB_PATH) -lController +MAIN_TARGET = $(LIB_PREFIX)car$(SHARED_LIB_EXTENSION) +LFLAGS = -shared -Wl,-rpath,'$$ORIGIN' + +ifeq ($(OSTYPE),linux) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.so +endif + +ifeq ($(OSTYPE),darwin) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.dylib +endif + +ifeq ($(OSTYPE),windows) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/Controller.dll +endif + +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/car.def b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/car.def new file mode 100644 index 000000000..1c3e82018 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/car.def @@ -0,0 +1,28 @@ +EXPORTS +wbu_car_init +wbu_car_cleanup +wbu_car_get_type +wbu_car_get_engine_type +wbu_car_set_indicator_period +wbu_car_get_indicator_period +wbu_car_get_backwards_lights +wbu_car_get_brake_lights +wbu_car_get_track_front +wbu_car_get_track_rear +wbu_car_get_wheelbase +wbu_car_get_front_wheel_radius +wbu_car_get_rear_wheel_radius +wbu_car_get_wheel_encoder +wbu_car_get_wheel_speed +wbu_car_set_right_steering_angle +wbu_car_get_right_steering_angle +wbu_car_set_left_steering_angle +wbu_car_get_left_steering_angle +wbu_car_enable_limited_slip_differential +wbu_car_enable_indicator_auto_disabling +_wbu_car_get_instance +_wbu_car_set_led_state_if_exist +_wbu_car_get_led_state_if_exist +_wbu_car_check_initialisation +_wbu_car_initialization_is_possible + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car.c b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car.c new file mode 100644 index 000000000..0f0e91c4e --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car.c @@ -0,0 +1,469 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * Description: Implementation of the car library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#include "car_private.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define DEFAULT_INDICATOR_PERIOD 0.34 +#ifdef _MSC_VER +#ifndef INFINITY +static const double tmp = 1.0; +#define INFINITY (1.0 / (tmp - 1.0)) +#endif +#endif + +static car *instance = NULL; +car *_wbu_car_get_instance() { + return instance; +} + +static const char *motorNames[] = {"right_steer", "left_steer", "right_front_wheel", + "left_front_wheel", "right_rear_wheel", "left_rear_wheel"}; + +static const char *wheelsSensorsNames[] = {"right_steer_sensor", "left_steer_sensor", "right_front_sensor", + "left_front_sensor", "right_rear_sensor", "left_rear_sensor"}; + +static const char *wheelsBrakesNames[] = {"right_front_brake", "left_front_brake", "right_rear_brake", "left_rear_brake"}; + +static const char *lightNames[NB_LIGHTS] = { + "front_lights", "antifog_lights", "right_indicators", "left_indicators", "rear_lights", + "backwards_lights", "brake_lights", "interior_right_indicators", "interior_left_indicators"}; + +static const char *displayNames[NB_MIRRORS] = {"rear_display", "left_wing_display", "right_wing_display"}; + +static const char *cameraNames[NB_MIRRORS] = {"rear_camera", "left_wing_camera", "right_wing_camera"}; + +static const char *steeringWheelName = "steering_wheel_motor"; + +static const char *indicatorLeverName = "indicator_lever_motor"; + +static const char *wiperMotorNames[NB_WIPERS] = {"right_wiper_motor", "left_wiper_motor"}; + +static const char *wiperSensorName = "wiper_sensor"; + +static const char *needleMotorNames[NB_NEEDLES] = {"rpm_needle_motor", "speed_needle_motor"}; + +//***********************************// +// Utility functions // +//***********************************// + +bool _wbu_car_check_initialisation(const char *init_function, const char *calling_function) { + if (instance == NULL) { + fprintf(stderr, "Warning: '%s' should be called before to use '%s'.\n", init_function, calling_function); + return false; + } else + return true; +} + +bool _wbu_car_get_led_state_if_exist(int index) { + if (index < NB_LIGHTS && instance->lights[index] != 0 && wb_led_get(instance->lights[index]) != 0) + return true; + else + return false; +} + +void _wbu_car_set_led_state_if_exist(int index, bool state) { + if (index < NB_LIGHTS && instance->lights[index] != 0) + wb_led_set(instance->lights[index], state); + if (index == RIGHT_INDICATOR_INDEX && instance->lights[INTERIOR_RIGHT_INDICATOR_INDEX] != 0) + wb_led_set(instance->lights[INTERIOR_RIGHT_INDICATOR_INDEX], state); + else if (index == LEFT_INDICATOR_INDEX && instance->lights[INTERIOR_LEFT_INDICATOR_INDEX] != 0) + wb_led_set(instance->lights[INTERIOR_LEFT_INDICATOR_INDEX], state); +} + +//***********************************// +// API functions // +//***********************************// + +void wbu_car_init() { + if (instance != NULL) + return; + + wb_robot_init(); + int i; + instance = (car *)malloc(sizeof(car)); + for (i = 0; i < 4; i++) { + instance->wheels[i] = 0; + instance->speeds[i] = 0.0; + } + for (i = 0; i < NB_LIGHTS; i++) + instance->lights[i] = 0; + for (i = 0; i < NB_MIRRORS; i++) { + instance->displays[i] = 0; + instance->cameras[i] = 0; + } + for (i = 0; i < NB_WIPERS; i++) + instance->wiper_motors[i] = 0; + for (i = 0; i < NB_NEEDLES; i++) + instance->needle_motors[i] = 0; + instance->steering_wheel = 0; + instance->indicator_lever_motor = 0; + instance->wiper_sensor = 0; + instance->indicator_period = DEFAULT_INDICATOR_PERIOD; + instance->right_angle = 0.0; + instance->left_angle = 0.0; + instance->limited_slip_differential = true; + instance->indicator_auto_disabling = true; + + // Parse vehicle caracteristics from the beginning of the data string + char engine_type; + int engine_sound_length; + char *sub_data_string = (char *)wb_robot_get_custom_data(); + i = sscanf(sub_data_string, "%lf %lf %lf %lf %lf %lf %lf %c %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %d", + &instance->wheelbase, &instance->track_front, &instance->track_rear, &instance->front_wheel_radius, + &instance->rear_wheel_radius, &instance->brake_coefficient, &instance->defaultDampingConstant, &engine_type, + &instance->engine_max_torque, &instance->engine_max_power, &instance->engine_min_rpm, &instance->engine_max_rpm, + &instance->engine_coefficients[0], &instance->engine_coefficients[1], &instance->engine_coefficients[2], + &instance->hybrid_power_split_ratio, &instance->hybrid_power_split_rpm, &instance->engine_sound_rpm_reference, + &instance->gear_number, &engine_sound_length); + + if (i < 20) { + fprintf(stderr, "Error: Only nodes based on the 'Car' node can use the car library.\n"); + exit(-1); + } + + // Extract engine type from the char + if (engine_type == 'E') + instance->engine_type = WBU_CAR_ELECTRIC_ENGINE; + else if (engine_type == 'P') + instance->engine_type = WBU_CAR_PARALLEL_HYBRID_ENGINE; + else if (engine_type == 'S') + instance->engine_type = WBU_CAR_POWER_SPLIT_HYBRID_ENGINE; + else + instance->engine_type = WBU_CAR_COMBUSTION_ENGINE; + + // Extract the gear ratio from the rest of the string (variable size) + instance->gear_ratio = (double *)malloc(instance->gear_number * sizeof(double)); + // skip first twenty parameters (beginning of the string already parsed) + for (i = 0; i < 20; ++i) + sub_data_string = strchr(sub_data_string, ' ') + 1; + // get all the gear ratio + for (i = 0; i < instance->gear_number; ++i) { + sscanf(sub_data_string, "%lf", &instance->gear_ratio[i]); + sub_data_string = strchr(sub_data_string, ' ') + 1; + } + // a minimum of two gear ratio is required + if (instance->gear_number < 2) { + fprintf(stderr, "Error: Any car should have a minimum of two gear ratios.\n"); + exit(-1); + } + + // Extract the engine sound file name + instance->engine_sound = (char *)malloc(engine_sound_length + 1); + memcpy(instance->engine_sound, sub_data_string, engine_sound_length); + instance->engine_sound[engine_sound_length] = '\0'; + + // Check for steering motors and position sensors + instance->steering_motors[0] = wb_robot_get_device(motorNames[0]); + instance->steering_motors[1] = wb_robot_get_device(motorNames[1]); + instance->steering_sensors[0] = wb_robot_get_device(wheelsSensorsNames[0]); + instance->steering_sensors[1] = wb_robot_get_device(wheelsSensorsNames[1]); + + if (instance->steering_motors[0] == 0 || instance->steering_motors[1] == 0) { + fprintf(stderr, "Error: Any car should have '%s' and '%s' motors.\n", motorNames[0], motorNames[1]); + exit(-1); + } + + if (instance->steering_sensors[0] == 0 || instance->steering_sensors[1] == 0) { + fprintf(stderr, "Error: Any car should have '%s' and '%s' position sensors.\n", wheelsSensorsNames[0], + wheelsSensorsNames[1]); + exit(-1); + } + wb_position_sensor_enable(instance->steering_sensors[0], (int)wb_robot_get_basic_time_step()); + wb_position_sensor_enable(instance->steering_sensors[1], (int)wb_robot_get_basic_time_step()); + + // Check for motors and lights + int n_devices = wb_robot_get_number_of_devices(); + for (i = 0; i < n_devices; i++) { + WbDeviceTag tag = wb_robot_get_device_by_index(i); + const char *name = wb_device_get_name(tag); + int j; + // Check for motors without warning + for (j = 0; j < 4; j++) { + if (strcmp(name, motorNames[j + 2]) == 0) + instance->wheels[j] = tag; + } + // Check for lights without warning + for (j = 0; j < NB_LIGHTS; j++) { + if (strcmp(name, lightNames[j]) == 0) + instance->lights[j] = tag; + } + // Check for cameras and displays without warning + for (j = 0; j < NB_MIRRORS; j++) { + if (strcmp(name, displayNames[j]) == 0) + instance->displays[j] = tag; + if (strcmp(name, cameraNames[j]) == 0) + instance->cameras[j] = tag; + } + // Check for steering wheel motor without warning + if (strcmp(name, steeringWheelName) == 0) + instance->steering_wheel = tag; + // Check for indicator lever motor without warning + if (strcmp(name, indicatorLeverName) == 0) + instance->indicator_lever_motor = tag; + + // Check for wiper motors and sensor without warning + for (j = 0; j < NB_WIPERS; j++) { + if (strcmp(name, wiperMotorNames[j]) == 0) + instance->wiper_motors[j] = tag; + } + if (strcmp(name, wiperSensorName) == 0) + instance->wiper_sensor = tag; + + // Check for needle motors without warning + for (j = 0; j < NB_NEEDLES; j++) { + if (strcmp(name, needleMotorNames[j]) == 0) + instance->needle_motors[j] = tag; + } + } + + // Check and enable for wiper sensor + if (instance->wiper_sensor != 0) + wb_position_sensor_enable(instance->wiper_sensor, (int)wb_robot_get_basic_time_step()); + + // Check for type + if (instance->wheels[0] != 0 && instance->wheels[1] != 0 && instance->wheels[2] != 0 && instance->wheels[3] != 0) + instance->type = WBU_CAR_FOUR_BY_FOUR; + else if (instance->wheels[0] != 0 && instance->wheels[1] != 0) + instance->type = WBU_CAR_TRACTION; + else if (instance->wheels[2] != 0 && instance->wheels[3] != 0) + instance->type = WBU_CAR_PROPULSION; + else { + fprintf(stderr, "Error: this car does not have the required motors\n"); + exit(-1); + } + + // Get positionSensors + for (i = 0; i < 4; i++) { + instance->sensors[i] = wb_robot_get_device(wheelsSensorsNames[i + 2]); + if (instance->sensors[i] == 0) { + fprintf(stderr, "Error: this car does not have the required position sensors\n"); + exit(-1); + } + } + + // Get Brake device + for (i = 0; i < 4; i++) { + instance->brakes[i] = wb_robot_get_device(wheelsBrakesNames[i]); + if (instance->brakes[i] == 0) { + fprintf(stderr, "Error: this car does not have the required brake devices\n"); + exit(-1); + } + } + + // Attach cameras to displays + for (i = 0; i < NB_MIRRORS; i++) { + if (instance->displays[i] != 0 && instance->cameras[i] != 0) { + int display_width = wb_display_get_width(instance->displays[i]); + int display_height = wb_display_get_height(instance->displays[i]); + int camera_width = wb_camera_get_width(instance->cameras[i]); + int camera_height = wb_camera_get_height(instance->cameras[i]); + if (display_width == camera_width && display_height == camera_height) { + int step = wb_robot_get_basic_time_step(); + while (step < 40) // maximum 25Hz at real-time + step += wb_robot_get_basic_time_step(); + wb_camera_enable(instance->cameras[i], step); + wb_display_attach_camera(instance->displays[i], instance->cameras[i]); + } + } + } + + // Set target position to infinite (we are not going to use control in position, but only speed and torque) + if (instance->type == WBU_CAR_TRACTION || instance->type == WBU_CAR_FOUR_BY_FOUR) { + wb_motor_set_position(instance->wheels[0], INFINITY); + wb_motor_set_position(instance->wheels[1], INFINITY); + wb_motor_set_velocity(instance->wheels[0], 0.0); + wb_motor_set_velocity(instance->wheels[1], 0.0); + } + if (instance->type == WBU_CAR_PROPULSION || instance->type == WBU_CAR_FOUR_BY_FOUR) { + wb_motor_set_position(instance->wheels[2], INFINITY); + wb_motor_set_position(instance->wheels[3], INFINITY); + wb_motor_set_velocity(instance->wheels[2], 0.0); + wb_motor_set_velocity(instance->wheels[3], 0.0); + } +} + +void wbu_car_cleanup() { + if (instance == NULL) + return; + if (instance->gear_ratio != NULL) { + free(instance->gear_ratio); + instance->gear_ratio = NULL; + } + if (instance->engine_sound != NULL) { + free(instance->engine_sound); + instance->engine_sound = NULL; + } + free(instance); + instance = NULL; + wb_robot_cleanup(); +} + +WbuCarType wbu_car_get_type() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_type()")) + return (WbuCarType)0; + return instance->type; +} + +WbuCarEngineType wbu_car_get_engine_type() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_engine_type()")) + return (WbuCarEngineType)0; + return instance->engine_type; +} + +double wbu_car_get_track_front() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_track_front()")) + return 0; + return instance->track_front; +} + +double wbu_car_get_track_rear() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_track_rear()")) + return 0; + return instance->track_rear; +} + +double wbu_car_get_wheelbase() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_wheelbase()")) + return 0; + return instance->wheelbase; +} + +double wbu_car_get_front_wheel_radius() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_front_wheel_radius()")) + return 0; + return instance->front_wheel_radius; +} + +double wbu_car_get_rear_wheel_radius() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_rear_wheel_radius()")) + return 0; + return instance->rear_wheel_radius; +} + +void wbu_car_set_indicator_period(double period) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_set_indicator_period()")) + return; + instance->indicator_period = period; +} + +double wbu_car_get_indicator_period() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_indicator_period()")) + return 0; + return instance->indicator_period; +} + +bool wbu_car_get_backwards_lights() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_backwards_lights()")) + return false; + return _wbu_car_get_led_state_if_exist(BACKWARDS_LIGHT_INDEX); +} + +bool wbu_car_get_brake_lights() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_brake_lights()")) + return false; + return _wbu_car_get_led_state_if_exist(BRAKE_LIGHT_INDEX); +} + +double wbu_car_get_wheel_encoder(WbuCarWheelIndex wheel_index) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_wheel_encoder()")) + return 0.0; + if (wheel_index >= WBU_CAR_WHEEL_NB) { + fprintf(stderr, "Warning: maximum wheel index is %d.\n", WBU_CAR_WHEEL_NB); + return 0.0; + } + // check if needed to be enable + if (wb_position_sensor_get_sampling_period(instance->sensors[wheel_index]) == 0) + wb_position_sensor_enable(instance->sensors[wheel_index], (int)wb_robot_get_basic_time_step()); + return wb_position_sensor_get_value(instance->sensors[wheel_index]); +} + +double wbu_car_get_wheel_speed(WbuCarWheelIndex wheel_index) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_wheel_speed()")) + return 0.0; + if (wheel_index >= WBU_CAR_WHEEL_NB) { + fprintf(stderr, "Warning: maximum wheel index is %d.\n", WBU_CAR_WHEEL_NB); + return 0.0; + } + return instance->speeds[wheel_index]; +} + +void wbu_car_set_right_steering_angle(double angle) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_set_right_steering_angle()")) + return; + + if (isnan(angle)) { + fprintf(stderr, "Warning: %s() called with an invalid 'angle' argument (NaN)\n", __FUNCTION__); + return; + } + + instance->right_angle = angle; + wb_motor_set_position(instance->steering_motors[0], angle); +} + +void wbu_car_set_left_steering_angle(double angle) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_set_left_steering_angle()")) + return; + + if (isnan(angle)) { + fprintf(stderr, "Warning: %s() called with an invalid 'angle' argument (NaN)\n", __FUNCTION__); + return; + } + + instance->left_angle = angle; + wb_motor_set_position(instance->steering_motors[1], angle); +} + +double wbu_car_get_right_steering_angle() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_right_steering_angle()")) + return 0.0; + return wb_position_sensor_get_value(instance->steering_sensors[0]); +} + +double wbu_car_get_left_steering_angle() { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_get_left_steering_angle()")) + return 0.0; + return wb_position_sensor_get_value(instance->steering_sensors[1]); +} + +void wbu_car_enable_limited_slip_differential(bool enable) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_limited_slip_differential()")) + return; + instance->limited_slip_differential = enable; +} + +void wbu_car_enable_indicator_auto_disabling(bool enable) { + if (!_wbu_car_check_initialisation("wbu_car_init()", "wbu_car_enable_indicator_auto_disabling()")) + return; + instance->indicator_auto_disabling = enable; +} diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car_private.h b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car_private.h new file mode 100644 index 000000000..e9fdc5357 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/car/src/car_private.h @@ -0,0 +1,93 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef CAR_PRIVATE_HPP +#define CAR_PRIVATE_HPP + +#include +#include + +#define NB_MIRRORS 3 +#define NB_WIPERS 2 +#define NB_NEEDLES 2 + +enum { + FRONT_LIGHT_INDEX, + ANTIFOG_LIGHT_INDEX, + RIGHT_INDICATOR_INDEX, + LEFT_INDICATOR_INDEX, + REAR_LIGHT_INDEX, + BACKWARDS_LIGHT_INDEX, + BRAKE_LIGHT_INDEX, + INTERIOR_RIGHT_INDICATOR_INDEX, + INTERIOR_LEFT_INDICATOR_INDEX, + NB_LIGHTS +} LIGHT_INDEX; + +typedef struct { + // Devices + WbDeviceTag steering_motors[2]; // refeer to steering Motors, 0: right, 1: left + WbDeviceTag steering_sensors[2]; // refeer to steering PositionSensors, 0: right, 1: left + WbDeviceTag wheels[4]; // refeer to Motors, 0: front_right, 1: front_left, 2: rear_right, 3: rear_left + WbDeviceTag sensors[4]; // refeer to PositionSensors, 0: front_right, 1: front_left, 2: rear_right, 3: rear_left + WbDeviceTag brakes[4]; // refeer to Brakes, 0: front_right, 1: front_left, 2: rear_right, 3: rear_left + WbDeviceTag lights[NB_LIGHTS]; // refeer to LEDs + WbDeviceTag displays[NB_MIRRORS]; // mirror displays + WbDeviceTag cameras[NB_MIRRORS]; // mirror camera + WbDeviceTag steering_wheel; // steering wheel motor + WbDeviceTag indicator_lever_motor; // indicator lever motor + WbDeviceTag wiper_motors[NB_WIPERS]; // windshield wiper motors, 0: right, 1: left + WbDeviceTag wiper_sensor; // windshield wiper sensor + WbDeviceTag needle_motors[NB_NEEDLES]; // speedometer needles, 0: right (rpm), 1: left (km/h) + // Commands + double indicator_period; + bool limited_slip_differential; + bool indicator_auto_disabling; + // Vehicle caracteristics + WbuCarType type; + WbuCarEngineType engine_type; + double track_front; + double track_rear; + double wheelbase; + double front_wheel_radius; + double rear_wheel_radius; + double brake_coefficient; + double defaultDampingConstant; + char *engine_sound; + double engine_sound_rpm_reference; + double engine_min_rpm; + double engine_max_rpm; + double engine_coefficients[3]; // combustion engine + double engine_max_torque; // electric engine + double engine_max_power; // electric engine + double hybrid_power_split_ratio; // hybrid power-split engine + double hybrid_power_split_rpm; // hybrid power-split engine + int gear_number; // including reverse + double *gear_ratio; // 0 is reverse + // Mesures + double speeds[4]; // rad/s, 0: front_right, 1: front_left, 2: rear_right, 3: rear_left + double max_acceleration; // current bigger acceleration of all the actuated wheels [rad/s2] + // Internal + double right_angle; + double left_angle; +} car; + +car *_wbu_car_get_instance(); +void _wbu_car_set_led_state_if_exist(int index, bool state); +bool _wbu_car_get_led_state_if_exist(int index); +bool _wbu_car_check_initialisation(const char *init_function, const char *calling_function); + +#endif // CAR_PRIVATE_HPP diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/Makefile new file mode 100644 index 000000000..ec3231b8f --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/Makefile @@ -0,0 +1,39 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +WEBOTS_LIBRARY = 1 +C_SOURCES = $(wildcard src/*.c) + +LIBRARIES = -L$(WEBOTS_CONTROLLER_LIB_PATH) -lcar +MAIN_TARGET = $(LIB_PREFIX)driver$(SHARED_LIB_EXTENSION) +LFLAGS = -shared -Wl,-rpath,'$$ORIGIN' + +ifeq ($(OSTYPE),linux) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libcar.so +endif + +ifeq ($(OSTYPE),darwin) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libcar.dylib +endif + +ifeq ($(OSTYPE),windows) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/car.dll +endif + +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/driver.def b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/driver.def new file mode 100644 index 000000000..20a14276f --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/driver.def @@ -0,0 +1,30 @@ +EXPORTS +wbu_driver_init +wbu_driver_initialization_is_possible +wbu_driver_cleanup +wbu_driver_step +wbu_driver_set_steering_angle +wbu_driver_get_steering_angle +wbu_driver_set_cruising_speed +wbu_driver_get_target_cruising_speed +wbu_driver_get_current_speed +wbu_driver_set_throttle +wbu_driver_get_throttle +wbu_driver_set_brake_intensity +wbu_driver_get_brake_intensity +wbu_driver_set_indicator +wbu_driver_set_hazard_flashers +wbu_driver_get_indicator +wbu_driver_get_hazard_flashers +wbu_driver_set_dipped_beams +wbu_driver_set_antifog_lights +wbu_driver_get_dipped_beams +wbu_driver_get_antifog_lights +wbu_driver_get_rpm +wbu_driver_get_gear +wbu_driver_set_gear +wbu_driver_get_gear_number +wbu_driver_get_control_mode +wbu_driver_set_wiper_mode +wbu_driver_get_wiper_mode + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/src/driver.c b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/src/driver.c new file mode 100644 index 000000000..b56dc9476 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/c/driver/src/driver.c @@ -0,0 +1,934 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +/* + * Description: Implementation of the driver library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ +#define _USE_MATH_DEFINES + +#ifdef _MSC_VER +#define isnan(x) ((x) != (x)) +#endif + +#ifdef _MSC_VER +#ifndef INFINITY +static const double tmp = 1.0; +#define INFINITY (1.0 / (tmp - 1.0)) +#endif +#endif + +#define THROTTLE_TO_VOLUME_GAIN 0.2 +#define RPM_TO_VOLUME_GAIN 0.5 + +#include "webots/vehicle/driver.h" + +#include +#include +#include +#include +#include +#include "../car/src/car_private.h" + +#include +#include +#include +#include + +#define ACCELERATION_THRESHOLD 50 // maximum acceleration allowed for the wheels [rad/s2] +#define INDICATOR_AUTO_DISABLING_THRESHOLD 0.1 +#define BLINKER_SOUND_FILE "sounds/blinker.wav" + +typedef struct { + // Car + car *car; + // Devices + WbDeviceTag engine_speaker; + // Commands + WbuDriverWiperMode wiper_mode; + WbuDriverIndicatorState indicator_state; + bool hazard_flashers_on; + double steering_angle; + double cruising_speed; + double throttle; + double brake; + int gear; + int dipped_beams_state; + // Mesures + double rpm; + // Internal + WbuDriverControlMode control_mode; + double front_slip_ratio; + double rear_slip_ratio; + double central_slip_ratio; + double basic_time_step; + double indicator_angle; // steering angle at the moment when indicator is switched on +} driver; + +static driver *instance = NULL; + +//***********************************// +// Utility functions // +//***********************************// + +bool wbu_driver_initialization_is_possible() { + // Parse vehicle caracteristics from the beginning of the data string + int read_int; + double read_double; + char read_char; + int i; + + wb_robot_init(); + const char *sub_data_string = wb_robot_get_custom_data(); + i = sscanf(sub_data_string, "%lf %lf %lf %lf %lf %lf %lf %c %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %d %d", &read_double, + &read_double, &read_double, &read_double, &read_double, &read_double, &read_double, &read_char, &read_double, + &read_double, &read_double, &read_double, &read_double, &read_double, &read_double, &read_double, &read_double, + &read_double, &read_int, &read_int); + + if (i < 20) + return false; + + return true; +} + +static double kmh_to_rads(double kmh, double wheel_radius) { + return kmh / 3.6 / wheel_radius; +} + +// compute wheels front geometric differential ratio right/left +static double differential_ratio_front() { + if (instance->steering_angle == 0) + return 0.5; + + double right_turning_radius = instance->car->wheelbase / sin(instance->car->right_angle); + double left_turning_radius = instance->car->wheelbase / sin(instance->car->left_angle); + + return right_turning_radius / (left_turning_radius + right_turning_radius); +} + +// compute wheels rear geometric differential ratio right/left +static double differential_ratio_rear() { + if (instance->steering_angle == 0) + return 0.5; + + double right_turning_radius = instance->car->wheelbase / tan(instance->steering_angle) - (instance->car->track_rear / 2); + double left_turning_radius = instance->car->wheelbase / tan(instance->steering_angle) + (instance->car->track_rear / 2); + + return right_turning_radius / (left_turning_radius + right_turning_radius); +} + +// compute central geometric differential ratio front/rear +static double differential_ratio_central() { + if (instance->steering_angle == 0) + return 0.5; + + double front_turning_radius = instance->car->wheelbase / sin(instance->steering_angle); + double rear_turning_radius = instance->car->wheelbase / tan(instance->steering_angle); + double wheels_radius_ratio = instance->car->rear_wheel_radius / instance->car->front_wheel_radius; + + return (wheels_radius_ratio * front_turning_radius) / (front_turning_radius + rear_turning_radius); +} + +static double compute_output_torque() { + // Compute available torque taking into account the current gear ratio and engine model + double gear_ratio; + if (instance->gear > 0) + gear_ratio = instance->car->gear_ratio[instance->gear]; + else if (instance->gear < 0) // reverse + gear_ratio = instance->car->gear_ratio[0]; + else + return 0.0; // no gear engaged + + double engine_torque = 0; + WbuCarEngineType engine = instance->car->engine_type; + + double real_rpm = instance->rpm; + // cppcheck-suppress knownConditionTrueFalse + if (instance->gear > 0 && real_rpm < 0) + real_rpm = 0; + double engine_rpm = real_rpm; + if ((engine_rpm < instance->car->engine_min_rpm) && + (engine == WBU_CAR_COMBUSTION_ENGINE || engine == WBU_CAR_PARALLEL_HYBRID_ENGINE || + engine == WBU_CAR_POWER_SPLIT_HYBRID_ENGINE)) + engine_rpm = instance->car->engine_min_rpm; + + // in case of parallel hybrid engine, the combustion engine is switch on only if min-rpm is reached + if (engine == WBU_CAR_COMBUSTION_ENGINE || (engine == WBU_CAR_PARALLEL_HYBRID_ENGINE && real_rpm == engine_rpm)) + engine_torque += instance->car->engine_coefficients[0] + engine_rpm * instance->car->engine_coefficients[1] + + engine_rpm * engine_rpm * instance->car->engine_coefficients[2]; + if (engine == WBU_CAR_POWER_SPLIT_HYBRID_ENGINE && real_rpm == engine_rpm) + engine_torque += + (1 - instance->car->hybrid_power_split_ratio) * + (instance->car->engine_coefficients[0] + instance->car->hybrid_power_split_rpm * instance->car->engine_coefficients[1] + + instance->car->hybrid_power_split_rpm * instance->car->hybrid_power_split_rpm * instance->car->engine_coefficients[2]); + if (engine == WBU_CAR_ELECTRIC_ENGINE || engine == WBU_CAR_PARALLEL_HYBRID_ENGINE || + engine == WBU_CAR_POWER_SPLIT_HYBRID_ENGINE) { + double temporary_engine_torque = (instance->car->engine_max_power * 60) / (2 * M_PI * real_rpm); + if (temporary_engine_torque > instance->car->engine_max_torque) + temporary_engine_torque = instance->car->engine_max_torque; + engine_torque += temporary_engine_torque; + } + + double output_torque = engine_torque * instance->throttle * gear_ratio; + + if (real_rpm == instance->car->engine_max_rpm) // maximum rotation speed of the motor, we don't want to increase it ! + output_torque = 0; + + // Limit torque if maximum acceleration is reached (in order to not have big jump of wheels speed when one of them temporarily + // does not touch the ground) + if (fabs(instance->car->max_acceleration) > ACCELERATION_THRESHOLD) + output_torque *= (ACCELERATION_THRESHOLD / fabs(instance->car->max_acceleration)); + + return output_torque; +} + +//***********************************// +// Step functions // +//***********************************// + +static void update_wheels_speed(int ms) { // Warning speed is wrong the first two steps + int i = 0; + instance->car->max_acceleration = 0.0; + + // Enable positionSensors if not already done + for (i = 0; i < 4; i++) { + if (wb_position_sensor_get_sampling_period(instance->car->sensors[i]) == 0) + wb_position_sensor_enable(instance->car->sensors[i], ms); + } + + // Get current position of the wheels + static double previous_position[4] = {0.0, 0.0, 0.0, 0.0}; + static double previous_speed[4] = {0.0, 0.0, 0.0, 0.0}; + double current_position[4]; + for (i = 0; i < 4; i++) + current_position[i] = wb_position_sensor_get_value(instance->car->sensors[i]); + + // Compute wheels speeds + for (i = 0; i < 4; i++) { + instance->car->speeds[i] = 1000 * (current_position[i] - previous_position[i]) / ms; + const double acceleration = 1000 * (instance->car->speeds[i] - previous_speed[i]) / ms; + if (fabs(acceleration) > fabs(instance->car->max_acceleration)) + instance->car->max_acceleration = acceleration; + previous_position[i] = current_position[i]; + previous_speed[i] = instance->car->speeds[i]; + } + + // Update speed_display needle + if (instance->car->needle_motors[1]) { + const double abs_speed = fabs(wbu_driver_get_current_speed()); + if (abs_speed <= 220) { // 220 is the max speed on the speedometer texture. + const double speed_needle_position = abs_speed * wb_motor_get_max_position(instance->car->needle_motors[1]) / 220; + wb_motor_set_position(instance->car->needle_motors[1], speed_needle_position); + } else + wb_motor_set_position(instance->car->needle_motors[1], wb_motor_get_max_position(instance->car->needle_motors[1])); + } +} + +static void compute_rpm() { + // average speed of the four wheels (rad/s) + double average_speed = + (instance->car->speeds[0] + instance->car->speeds[1] + instance->car->speeds[2] + instance->car->speeds[3]) / 4; + // convert to rot/min + average_speed *= 60 / (2 * M_PI); + // convert to engine rpm + if (instance->gear == 0) { + instance->rpm = 0; // TODO: eventually improve this in a future version of the library + return; + } + + double gear_ratio = 0.0; + + if (instance->gear > 0) + gear_ratio = instance->car->gear_ratio[instance->gear]; + else if (instance->gear < 0) + gear_ratio = -instance->car->gear_ratio[0]; + + double max_rpm = instance->car->engine_max_rpm; + + // multiplication by the gear_ratio because we want motor rpm speed from wheels rpm + double rpm = fabs(average_speed * gear_ratio); + + if (isnan(rpm)) // in order to handle the first time step + rpm = 0; + else if (rpm > max_rpm) + rpm = max_rpm; + instance->rpm = rpm; + + // Update speed_display RPM needle + if (instance->car->needle_motors[0]) { + if (rpm <= 11000) { // 11 * 1000 is the max RPM on the speed_display texture. + const double rpm_needle_position = rpm * wb_motor_get_max_position(instance->car->needle_motors[0]) / 11000; + wb_motor_set_position(instance->car->needle_motors[0], rpm_needle_position); + } else + wb_motor_set_position(instance->car->needle_motors[0], wb_motor_get_max_position(instance->car->needle_motors[0])); + } +} + +static void update_slip_ratio() { + if (instance->car->type == WBU_CAR_TRACTION || instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + // Compute and update the front slip differential ratio (between -1 and 1) + double real_front_ratio = (instance->car->speeds[0] / (instance->car->speeds[0] + instance->car->speeds[1])) * 2 - 1; + // for better result a PD controller can be used here + instance->front_slip_ratio = instance->front_slip_ratio + real_front_ratio; + if (isnan(instance->front_slip_ratio)) + instance->front_slip_ratio = 0.0; + else if (instance->front_slip_ratio < -1) + instance->front_slip_ratio = -1; + else if (instance->front_slip_ratio > 1) + instance->front_slip_ratio = 1; + } + if (instance->car->type == WBU_CAR_PROPULSION || instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + // Compute and update the rear slip differential ratio (between -1 and 1) + double real_rear_ratio = (instance->car->speeds[2] / (instance->car->speeds[2] + instance->car->speeds[3])) * 2 - 1; + // for better result a PD controller can be used here + instance->rear_slip_ratio = instance->rear_slip_ratio + real_rear_ratio; + if (isnan(instance->rear_slip_ratio)) + instance->rear_slip_ratio = 0.0; + else if (instance->rear_slip_ratio < -1) + instance->rear_slip_ratio = -1; + else if (instance->rear_slip_ratio > 1) + instance->rear_slip_ratio = 1; + } + if (instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + // Compute and update the central slip differential ratio (between -1 and 1) + double front_speed_sum = instance->car->speeds[0] + instance->car->speeds[1]; + double rear_speed_sum = instance->car->speeds[2] + instance->car->speeds[3]; + double real_central_ratio = (front_speed_sum / (front_speed_sum + rear_speed_sum)) * 2 - 1; + // for better result a PD controller can be used here + instance->central_slip_ratio = instance->central_slip_ratio + real_central_ratio; + if (isnan(instance->central_slip_ratio)) + instance->central_slip_ratio = 0.0; + else if (instance->central_slip_ratio < -1) + instance->central_slip_ratio = -1; + else if (instance->central_slip_ratio > 1) + instance->central_slip_ratio = 1; + } +} + +static void update_torque() { + double torque = compute_output_torque(); + + // Distribute the available torque to the actuated wheels using 'geometric' differential rules + if (instance->car->type == WBU_CAR_TRACTION) { + // Geometric differential ratio (left and right wheel should not have same torque because the rotation radius is not the + // same) + double ratio = differential_ratio_front(); + // Add the limited differential slip if enable + // the formula is just in order to keep ratio between 0 and 1 + if (instance->car->limited_slip_differential) + ratio *= 2 * (1 - ((instance->front_slip_ratio + 1) / 2)); + // Compute and apply torques + double right_torque = torque * ratio; + double left_torque = torque * (1 - ratio); + wb_motor_set_torque(instance->car->wheels[0], right_torque); + wb_motor_set_torque(instance->car->wheels[1], left_torque); + + } else if (instance->car->type == WBU_CAR_PROPULSION) { + // Geometric differential ratio (left and right wheel should not have same torque because the rotation radius is not the + // same) + double ratio = differential_ratio_rear(); + // Add the limited differential slip if enable + // the formula is just in order to keep ratio between 0 and 1 + if (instance->car->limited_slip_differential) + ratio *= 2 * (1 - ((instance->rear_slip_ratio + 1) / 2)); + // Compute and apply torques + double right_torque = torque * ratio; + double left_torque = torque * (1 - ratio); + wb_motor_set_torque(instance->car->wheels[2], right_torque); + wb_motor_set_torque(instance->car->wheels[3], left_torque); + + } else if (instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + // Geometric differential ratio + double ratio_front = differential_ratio_front(); + double ratio_rear = differential_ratio_rear(); + double ratio_central = differential_ratio_central(); + // Add the limited differential slip if enable + if (instance->car->limited_slip_differential) { + // the formula is just in order to keep ratio between 0 and 1 + ratio_front *= 2 * (1 - ((instance->front_slip_ratio + 1) / 2)); + ratio_rear *= 2 * (1 - ((instance->rear_slip_ratio + 1) / 2)); + ratio_central *= 2 * (1 - ((instance->central_slip_ratio + 1) / 2)); + } + + // Compute and apply torques + double front_right_torque = torque * ratio_front * ratio_central; + double front_left_torque = torque * (1 - ratio_front) * ratio_central; + double rear_right_torque = torque * ratio_rear * (1 - ratio_central); + double rear_left_torque = torque * (1 - ratio_rear) * (1 - ratio_central); + wb_motor_set_torque(instance->car->wheels[0], front_right_torque); + wb_motor_set_torque(instance->car->wheels[1], front_left_torque); + wb_motor_set_torque(instance->car->wheels[2], rear_right_torque); + wb_motor_set_torque(instance->car->wheels[3], rear_left_torque); + } +} + +static void update_brake() { + // Compute and apply dampingConstant + int i; + for (i = 0; i < 4; i++) { + // we divide by the rotational speed of the wheels because we want to remove the dependency of the damping friction to the + // rotational speed + const double damping_constant = fabs(instance->brake * instance->car->brake_coefficient / instance->car->speeds[i]); + if (!isnan(damping_constant)) + wb_brake_set_damping_constant(instance->car->brakes[i], damping_constant); + } +} + +static void update_engine_sound() { + if (strcmp(instance->car->engine_sound, "none") != 0) { + double volume = 1.0; + bool stop_sound = false; + double rpm = 0; + if (instance->control_mode == TORQUE) { + rpm = instance->rpm; + // modulate volume by the throttle position + volume *= (1.0 - THROTTLE_TO_VOLUME_GAIN) + THROTTLE_TO_VOLUME_GAIN * instance->throttle; + // modulate volume by the rpm + volume *= (1.0 - RPM_TO_VOLUME_GAIN) + RPM_TO_VOLUME_GAIN * + (rpm > instance->car->engine_max_rpm ? instance->car->engine_max_rpm : rpm) / + instance->car->engine_max_rpm; + } else if (instance->control_mode == SPEED) { + // in speed mode, the rpm is estimated from the speed + // average speed of the four wheels (rad/s) + double average_speed = + (instance->car->speeds[0] + instance->car->speeds[1] + instance->car->speeds[2] + instance->car->speeds[3]) / 4; + if (isnan(average_speed)) // first step + return; + // convert to rot/min + average_speed *= 60 / (2 * M_PI); + // check which gear is the most appropriate + double gear_ratio = 0; + if (average_speed < 0) + gear_ratio = -instance->car->gear_ratio[0]; + else { + int i; + for (i = 1; i < instance->car->gear_number; ++i) { + gear_ratio = instance->car->gear_ratio[i]; + // the 'correct' gear is when rpm < 1 / 3 rpm range + if (fabs(average_speed * gear_ratio) < (2 * instance->car->engine_min_rpm + instance->car->engine_max_rpm) / 3) + break; + } + } + rpm = fabs(average_speed * gear_ratio); + // modulate volume by the difference between current speed and target speed (estimation of throttle) + double speed_diff = instance->cruising_speed - wbu_driver_get_current_speed(); + if (speed_diff < 0) // deceleration => no throttle + speed_diff = 0.0; + else if (speed_diff > 25.0) // we assume full throttle as soon as the difference is bigger than 25 + speed_diff = 25.0; + volume *= (1.0 - THROTTLE_TO_VOLUME_GAIN) + THROTTLE_TO_VOLUME_GAIN * speed_diff / 25.0; + // modulate volume by the rpm + volume *= (1.0 - RPM_TO_VOLUME_GAIN) + RPM_TO_VOLUME_GAIN * + (rpm < instance->car->engine_max_rpm ? instance->car->engine_max_rpm : rpm) / + instance->car->engine_max_rpm; + } + WbuCarEngineType engine = instance->car->engine_type; + if (engine == WBU_CAR_POWER_SPLIT_HYBRID_ENGINE) { + if (rpm > instance->car->engine_min_rpm) + rpm = instance->car->hybrid_power_split_rpm; + else + stop_sound = true; + } else if (engine == WBU_CAR_PARALLEL_HYBRID_ENGINE) { + if (rpm < instance->car->engine_min_rpm) + stop_sound = true; + } + if (rpm < instance->car->engine_min_rpm) + rpm = instance->car->engine_min_rpm; + double pitch = rpm / instance->car->engine_sound_rpm_reference; + if (stop_sound) + wb_speaker_stop(instance->engine_speaker, instance->car->engine_sound); + else + wb_speaker_play_sound(instance->engine_speaker, instance->engine_speaker, instance->car->engine_sound, volume, pitch, 0.0, + true); + } +} + +//***********************************// +// API functions // +//***********************************// + +void wbu_driver_init() { + if (instance != NULL) + return; + + wbu_car_init(); + instance = (driver *)malloc(sizeof(driver)); + instance->car = _wbu_car_get_instance(); + instance->wiper_mode = DOWN; + instance->indicator_state = OFF; + instance->hazard_flashers_on = false; + instance->steering_angle = 0.0; + instance->cruising_speed = 0.0; + instance->throttle = 0.0; + instance->brake = 0.0; + instance->gear = 0; + instance->dipped_beams_state = 0; + instance->rpm = 0.0; + instance->control_mode = UNDEFINED_CONTROL_MODE; + instance->front_slip_ratio = 0.0; + instance->rear_slip_ratio = 0.0; + instance->central_slip_ratio = 0.0; + instance->basic_time_step = wb_robot_get_basic_time_step(); + instance->indicator_angle = 0.0; + + instance->engine_speaker = wb_robot_get_device("engine_speaker"); + if (instance->engine_speaker == 0) { + fprintf(stderr, "Warning: Any car should have a 'engine_speaker' speaker.\n"); + exit(0); + } +} + +int wbu_driver_step() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_step()")) + return 0; + + // indicator + bool on = (((int)(wb_robot_get_time() / instance->car->indicator_period) % 2) == 0); + bool right_indicator = false, left_indicator = false; + if (instance->hazard_flashers_on) { // hazard flashers mode + right_indicator = on; + left_indicator = on; + } else if (instance->indicator_state == RIGHT) // right mode + right_indicator = on; + else if (instance->indicator_state == LEFT) // left mode + left_indicator = on; + + _wbu_car_set_led_state_if_exist(RIGHT_INDICATOR_INDEX, right_indicator); + _wbu_car_set_led_state_if_exist(LEFT_INDICATOR_INDEX, left_indicator); + + // wiper + if ((instance->car->wiper_motors[0] != 0 && instance->car->wiper_motors[1] != 0 && instance->car->wiper_sensor != 0) && + instance->wiper_mode != DOWN) { + bool stop = ((int)(wb_robot_get_time()) % 4) != 0; // allows for wiper in slow mode to be activated once every 4 seconds. + if (wb_position_sensor_get_value(instance->car->wiper_sensor) == + wb_motor_get_min_position(instance->car->wiper_motors[0])) { // wiper go up + wb_motor_set_position(instance->car->wiper_motors[0], wb_motor_get_max_position(instance->car->wiper_motors[0])); + wb_motor_set_position(instance->car->wiper_motors[1], wb_motor_get_max_position(instance->car->wiper_motors[1])); + } + if (wb_position_sensor_get_value(instance->car->wiper_sensor) == + wb_motor_get_max_position(instance->car->wiper_motors[0]) || + (instance->wiper_mode == SLOW && stop)) { + // wiper go/stay down + wb_motor_set_position(instance->car->wiper_motors[0], wb_motor_get_min_position(instance->car->wiper_motors[0])); + wb_motor_set_position(instance->car->wiper_motors[1], wb_motor_get_min_position(instance->car->wiper_motors[1])); + } + } + + // backward light on if control in torque is enable and gear -1 is engaged or control in speed is enable with a negative speed + if (((instance->control_mode == TORQUE) && (instance->gear == -1)) || + ((instance->control_mode == SPEED) && (instance->cruising_speed < 0))) + _wbu_car_set_led_state_if_exist(BACKWARDS_LIGHT_INDEX, true); + else + _wbu_car_set_led_state_if_exist(BACKWARDS_LIGHT_INDEX, false); + + // update wheels speed and compute engine rpm + update_wheels_speed((int)instance->basic_time_step); + compute_rpm(); + // update torque if control in torque is activated + if (instance->control_mode == TORQUE) { + // update the differential slip ratio if limited differential slip is enable + if (instance->car->limited_slip_differential) + update_slip_ratio(); + update_torque(); + } + + // update brake (dependant of the rotation speed of the wheels) + if (instance->brake > 0.0) + update_brake(); + + // update engine sound + update_engine_sound(); + + return wb_robot_step((int)instance->basic_time_step); +} + +void wbu_driver_cleanup() { + if (instance == NULL) + return; + free(instance); + instance = NULL; + wbu_car_cleanup(); +} + +void wbu_driver_set_steering_angle(double steering_angle) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_steering_angle()")) + return; + + if (isnan(steering_angle)) { + fprintf(stderr, "Warning: wbu_driver_set_steering_angle() called with an invalid 'steering_angle' argument (NaN)\n"); + return; + } + + // move the steering wheel (if any) + if (instance->car->steering_wheel != 0) + wb_motor_set_position(instance->car->steering_wheel, steering_angle * 10); + + // Steering mechanism, formulas from: + // Reza N. Jazar, Vehicle Dynamics: Theory and Application. Springer 2009 + instance->steering_angle = steering_angle; + double cot = 1 / tan(steering_angle); + double right_angle = atan(1 / (cot - (instance->car->track_front / (2 * instance->car->wheelbase)))); + double left_angle = atan(1 / (cot + (instance->car->track_front / (2 * instance->car->wheelbase)))); + instance->car->right_angle = right_angle; + instance->car->left_angle = left_angle; + wb_motor_set_position(instance->car->steering_motors[0], right_angle); // right + wb_motor_set_position(instance->car->steering_motors[1], left_angle); // left + + // the differential speeds need to be recomputed + if (instance->control_mode == SPEED) + wbu_driver_set_cruising_speed(instance->cruising_speed); + + // indicator auto-disabling mechanism + if (instance->car->indicator_auto_disabling) { + if (instance->indicator_state == RIGHT) { + if (steering_angle > instance->indicator_angle) // continue steering in the direction of the blinker + instance->indicator_angle = steering_angle; + else if (steering_angle - instance->indicator_angle <= -INDICATOR_AUTO_DISABLING_THRESHOLD) { + if (instance->car->indicator_lever_motor != 0) + wb_motor_set_position(instance->car->indicator_lever_motor, 0.0); + instance->indicator_state = OFF; + if (!instance->hazard_flashers_on) + wb_speaker_stop(instance->engine_speaker, BLINKER_SOUND_FILE); + } + } else if (instance->indicator_state == LEFT) { + if (steering_angle < instance->indicator_angle) // continue steering in the direction of the blinker + instance->indicator_angle = steering_angle; + else if (instance->indicator_angle - steering_angle <= -INDICATOR_AUTO_DISABLING_THRESHOLD) { + if (instance->car->indicator_lever_motor != 0) + wb_motor_set_position(instance->car->indicator_lever_motor, 0.0); + instance->indicator_state = OFF; + if (!instance->hazard_flashers_on) + wb_speaker_stop(instance->engine_speaker, BLINKER_SOUND_FILE); + } + } + } +} + +double wbu_driver_get_steering_angle() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_steering_angle()")) + return 0; + double right_angle = wb_position_sensor_get_value(instance->car->steering_sensors[0]); + double left_angle = wb_position_sensor_get_value(instance->car->steering_sensors[1]); + return (right_angle + left_angle) / 2.0; +} + +void wbu_driver_set_cruising_speed(double speed) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_cruising_speed()")) + return; + + // Compute and apply speeds of front wheels (if needed) + if (instance->car->type == WBU_CAR_TRACTION || instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + double ratio = differential_ratio_front(); + double right_speed = 2 * speed * ratio; + double left_speed = 2 * speed * (1 - ratio); + + // apply central differential if needed + if (instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + ratio = differential_ratio_central(); + right_speed *= 2 * ratio; + left_speed *= 2 * ratio; + } + + if (instance->car->front_wheel_radius == -1.0) { + fprintf(stderr, "Warning: wheel radius cannot be retrieve automatically.\n"); + } else { + wb_motor_set_velocity(instance->car->wheels[0], kmh_to_rads(right_speed, instance->car->front_wheel_radius)); + wb_motor_set_velocity(instance->car->wheels[1], kmh_to_rads(left_speed, instance->car->front_wheel_radius)); + if (instance->control_mode == TORQUE) { + // if control in torque was previously enable we need to reset the target position to infinite + wb_motor_set_position(instance->car->wheels[0], INFINITY); + wb_motor_set_position(instance->car->wheels[1], INFINITY); + } + } + } + + // Compute and apply speeds of rear wheels (if needed) + if (instance->car->type == WBU_CAR_PROPULSION || instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + double ratio = differential_ratio_rear(); + double right_speed = 2 * speed * ratio; + double left_speed = 2 * speed * (1 - ratio); + + // apply central differential if needed + if (instance->car->type == WBU_CAR_FOUR_BY_FOUR) { + ratio = differential_ratio_central(); + right_speed *= 2 * (1 - ratio); + left_speed *= 2 * (1 - ratio); + } + + if (instance->car->rear_wheel_radius == -1.0) { + fprintf(stderr, "Warning: wheel radius cannot be retrieve automatically.\n"); + } else { + wb_motor_set_velocity(instance->car->wheels[2], kmh_to_rads(right_speed, instance->car->rear_wheel_radius)); + wb_motor_set_velocity(instance->car->wheels[3], kmh_to_rads(left_speed, instance->car->rear_wheel_radius)); + if (instance->control_mode == TORQUE) { + // if control in torque was previously enable we need to reset the target position to infinite + wb_motor_set_position(instance->car->wheels[2], INFINITY); + wb_motor_set_position(instance->car->wheels[3], INFINITY); + } + } + } + + instance->cruising_speed = speed; + instance->control_mode = SPEED; +} + +double wbu_driver_get_target_cruising_speed() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_target_cruising_speed()")) + return 0; + return instance->cruising_speed; +} + +double wbu_driver_get_current_speed() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_current_speed()")) + return 0.0; + double speed = 0.0; + + switch (instance->car->type) { + case WBU_CAR_TRACTION: + if (instance->car->front_wheel_radius == -1.0) + fprintf(stderr, "Warning: wheel radius cannot be retrieve automatically.\n"); + else + speed = 0.5 * (instance->car->speeds[0] + instance->car->speeds[1]) * instance->car->front_wheel_radius; + break; + case WBU_CAR_FOUR_BY_FOUR: + if (instance->car->front_wheel_radius == -1.0 || instance->car->rear_wheel_radius == -1.0) + fprintf(stderr, "Warning: wheel radius cannot be retrieve automatically.\n"); + else + speed = 0.25 * (instance->car->speeds[0] + instance->car->speeds[1]) * instance->car->front_wheel_radius + + 0.25 * (instance->car->speeds[2] + instance->car->speeds[3]) * instance->car->rear_wheel_radius; + break; + case WBU_CAR_PROPULSION: + if (instance->car->rear_wheel_radius == -1.0) + fprintf(stderr, "Warning: wheel radius cannot be retrieve automatically.\n"); + else + speed = 0.5 * (instance->car->speeds[2] + instance->car->speeds[3]) * instance->car->rear_wheel_radius; + break; + } + speed *= 3.6; + + return speed; +} + +void wbu_driver_set_throttle(double throttle) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_throttle()")) + return; + if (throttle < 0) { + fprintf(stderr, "Warning: 'throttle' should be bigger or equal to 0, used 0 instead.\n"); + throttle = 0; + } else if (throttle > 1) { + fprintf(stderr, "Warning: 'throttle' should be smaller or equal to 1, used 1 instead.\n"); + throttle = 1; + } + + instance->throttle = throttle; + instance->control_mode = TORQUE; +} + +double wbu_driver_get_throttle() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_throttle()")) + return 0; + return instance->throttle; +} + +void wbu_driver_set_brake_intensity(double intensity) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_brake_intensity()")) + return; + if (intensity < 0.0) { + fprintf(stderr, "Warning: 'intensity' should be bigger or equal to 0, used 0 instead.\n"); + intensity = 0.0; + } else if (intensity > 1.0) { + fprintf(stderr, "Warning: 'intensity' should be smaller or equal to 1, used 1 instead.\n"); + intensity = 1.0; + } + instance->brake = intensity; + + // Update braking light + if (intensity > 0.0) + _wbu_car_set_led_state_if_exist(BRAKE_LIGHT_INDEX, true); + else { + _wbu_car_set_led_state_if_exist(BRAKE_LIGHT_INDEX, false); + int i; + // restore default damping constant + for (i = 0; i < 4; i++) + wb_brake_set_damping_constant(instance->car->brakes[i], 0.0); + } +} + +double wbu_driver_get_brake_intensity() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_brake_intensity()")) + return 0; + return instance->brake; +} + +void wbu_driver_set_brake(double brake) { + fprintf(stderr, "Warning: Deprecated 'wbu_driver_set_brake' use 'wbu_driver_set_brake_intensity' instead.\n"); + wbu_driver_set_brake_intensity(brake); +} + +double wbu_driver_get_brake() { + fprintf(stderr, "Warning: Deprecated 'wbu_driver_get_brake' use 'wbu_driver_get_brake_intensity' instead.\n"); + return wbu_driver_get_brake_intensity(); +} + +void wbu_driver_set_indicator(WbuDriverIndicatorState state) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_indicator()")) + return; + instance->indicator_state = state; + instance->indicator_angle = instance->steering_angle; + if (instance->indicator_state == RIGHT && instance->car->indicator_lever_motor != 0) + wb_motor_set_position(instance->car->indicator_lever_motor, + wb_motor_get_max_position(instance->car->indicator_lever_motor)); + else if (instance->indicator_state == LEFT && instance->car->indicator_lever_motor != 0) + wb_motor_set_position(instance->car->indicator_lever_motor, + wb_motor_get_min_position(instance->car->indicator_lever_motor)); + else if (instance->car->indicator_lever_motor != 0) + wb_motor_set_position(instance->car->indicator_lever_motor, 0.0); + + if (instance->hazard_flashers_on || instance->indicator_state != OFF) + wb_speaker_play_sound(instance->engine_speaker, instance->engine_speaker, BLINKER_SOUND_FILE, 1.0, 1.0, 0.0, true); + else + wb_speaker_stop(instance->engine_speaker, BLINKER_SOUND_FILE); +} + +void wbu_driver_set_hazard_flashers(bool state) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_hazard_flashers")) + return; + instance->hazard_flashers_on = state; + if (instance->hazard_flashers_on || instance->indicator_state != OFF) + wb_speaker_play_sound(instance->engine_speaker, instance->engine_speaker, BLINKER_SOUND_FILE, 1.0, 1.0, 0.0, true); + else + wb_speaker_stop(instance->engine_speaker, BLINKER_SOUND_FILE); +} + +WbuDriverIndicatorState wbu_driver_get_indicator() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_indicator()")) + return OFF; + return instance->indicator_state; +} + +bool wbu_driver_get_hazard_flashers() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_hazard_flashers")) + return false; + return instance->hazard_flashers_on; +} + +void wbu_driver_set_dipped_beams(bool state) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_dipped_beams()")) + return; + _wbu_car_set_led_state_if_exist(FRONT_LIGHT_INDEX, state); + _wbu_car_set_led_state_if_exist(REAR_LIGHT_INDEX, state); + instance->dipped_beams_state = state; +} + +void wbu_driver_set_antifog_lights(bool state) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_antifog_lights()")) + return; + _wbu_car_set_led_state_if_exist(ANTIFOG_LIGHT_INDEX, state); +} + +bool wbu_driver_get_dipped_beams() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_dipped_beams()")) + return false; + return instance->dipped_beams_state; +} + +bool wbu_driver_get_antifog_lights() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_antifog_lights()")) + return false; + return _wbu_car_get_led_state_if_exist(ANTIFOG_LIGHT_INDEX); +} + +double wbu_driver_get_rpm() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_rpm()")) + return 0.0; + if (instance->control_mode == SPEED) { + fprintf( + stderr, + "Warning: no engine model when cruise control is enable, call to 'wbu_driver_get_rpm()' is therefore not allowed.\n"); + return 0.0; + } + return instance->rpm; +} + +int wbu_driver_get_gear() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_gear()")) + return 0; + if (instance->control_mode == SPEED) { + fprintf( + stderr, + "Warning: no gear model when cruise control is enable, call to 'wbu_driver_get_gear()' is therefore not allowed.\n"); + return 0; + } + return instance->gear; +} + +void wbu_driver_set_gear(int gear) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_gear()")) + return; + if (gear > instance->car->gear_number) { + fprintf(stderr, "Warning: this car has only %d gears.\n", instance->car->gear_number); + gear = instance->car->gear_number; + } else if (gear < -1) { + fprintf(stderr, "Warning: Minimum gear value is -1"); + gear = -1; + } + instance->gear = gear; + return; +} + +int wbu_driver_get_gear_number() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_gear_number()")) + return 0; + return instance->car->gear_number; +} + +WbuDriverControlMode wbu_driver_get_control_mode() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_control_mode()")) + return UNDEFINED_CONTROL_MODE; + return instance->control_mode; +} + +WbuDriverWiperMode wbu_driver_get_wiper_mode() { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_get_wiper_mode()")) + return (WbuDriverWiperMode)-1; + return instance->wiper_mode; +} + +void wbu_driver_set_wiper_mode(WbuDriverWiperMode mode) { + if (!_wbu_car_check_initialisation("wbu_driver_init()", "wbu_driver_set_wiper_mode()")) + return; + if (instance->car->wiper_motors[0] != 0 && instance->car->wiper_motors[1] != 0) { + instance->wiper_mode = mode; + double speed = 2.0; // same speed for all other modes in order to be able to reset wiper position + if (mode == FAST) + speed = 4.0; + wb_motor_set_velocity(instance->car->wiper_motors[0], speed); + wb_motor_set_velocity(instance->car->wiper_motors[1], speed); + if (mode == DOWN) { + wb_motor_set_position(instance->car->wiper_motors[0], wb_motor_get_min_position(instance->car->wiper_motors[0])); + wb_motor_set_position(instance->car->wiper_motors[1], wb_motor_get_min_position(instance->car->wiper_motors[1])); + } + }; +} + +wbu_wipers_mode wbu_driver_get_wipers_mode() { + return wbu_driver_get_wiper_mode(); +} + +void wbu_driver_set_wipers_mode(wbu_wipers_mode mode) { + wbu_driver_set_wiper_mode(mode); +} diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/Makefile new file mode 100644 index 000000000..3faa227c0 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/Makefile @@ -0,0 +1,39 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +WEBOTS_LIBRARY = 1 +CXX_SOURCES = $(wildcard src/*.cpp) + +INCLUDE = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include/controller/cpp +MAIN_TARGET = $(LIB_PREFIX)CppCar$(SHARED_LIB_EXTENSION) +LIBRARIES = -L$(WEBOTS_CONTROLLER_LIB_PATH) -lcar -lCppDriver + +ifeq ($(OSTYPE),linux) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libcar.so $(WEBOTS_CONTROLLER_LIB_PATH)/libCppDriver.so +endif + +ifeq ($(OSTYPE),darwin) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libcar.dylib $(WEBOTS_CONTROLLER_LIB_PATH)/libCppDriver.dylib +endif + +ifeq ($(OSTYPE),windows) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/car.dll $(WEBOTS_CONTROLLER_LIB_PATH)/CppDriver.dll +endif + +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/src/Car.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/src/Car.cpp new file mode 100644 index 000000000..155330db9 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/car/src/Car.cpp @@ -0,0 +1,121 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Description: CPP wrapper of the car library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#include +#include + +#include + +using namespace webots; + +Car::Type Car::getType() { + assert(this); + return Type(wbu_car_get_type()); +} + +Car::EngineType Car::getEngineType() { + assert(this); + return EngineType(wbu_car_get_engine_type()); +} + +void Car::setIndicatorPeriod(double period) { + assert(this); + wbu_car_set_indicator_period(period); +} + +double Car::getIndicatorPeriod() { + assert(this); + return wbu_car_get_indicator_period(); +} + +bool Car::getBackwardsLights() { + assert(this); + return wbu_car_get_backwards_lights(); +} + +bool Car::getBrakeLights() { + assert(this); + return wbu_car_get_brake_lights(); +} + +double Car::getTrackFront() { + assert(this); + return wbu_car_get_track_front(); +} + +double Car::getTrackRear() { + assert(this); + return wbu_car_get_track_rear(); +} + +double Car::getWheelbase() { + assert(this); + return wbu_car_get_wheelbase(); +} + +double Car::getFrontWheelRadius() { + assert(this); + return wbu_car_get_front_wheel_radius(); +} + +double Car::getRearWheelRadius() { + assert(this); + return wbu_car_get_rear_wheel_radius(); +} + +double Car::getWheelEncoder(WheelIndex wheel) { + assert(this); + return wbu_car_get_wheel_encoder(WbuCarWheelIndex(wheel)); +} + +double Car::getWheelSpeed(WheelIndex wheel) { + assert(this); + return wbu_car_get_wheel_speed(WbuCarWheelIndex(wheel)); +} + +void Car::setRightSteeringAngle(double angle) { + assert(this); + wbu_car_set_right_steering_angle(angle); +} + +void Car::setLeftSteeringAngle(double angle) { + assert(this); + wbu_car_set_left_steering_angle(angle); +} + +double Car::getRightSteeringAngle() { + assert(this); + return wbu_car_get_right_steering_angle(); +} + +double Car::getLeftSteeringAngle() { + assert(this); + return wbu_car_get_left_steering_angle(); +} + +void Car::enableLimitedSlipDifferential(bool enable) { + assert(this); + wbu_car_enable_limited_slip_differential(enable); +} + +void Car::enableIndicatorAutoDisabling(bool enable) { + assert(this); + wbu_car_enable_indicator_auto_disabling(enable); +} diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/Makefile new file mode 100644 index 000000000..a9f33d257 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/Makefile @@ -0,0 +1,40 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +WEBOTS_LIBRARY = 1 +CXX_SOURCES = $(wildcard src/*.cpp) + +INCLUDE = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include/controller/cpp +MAIN_TARGET = $(LIB_PREFIX)CppDriver$(SHARED_LIB_EXTENSION) +LIBRARIES = -L$(WEBOTS_CONTROLLER_LIB_PATH) -ldriver -lCppController +LFLAGS = -shared -Wl,-rpath,'$$ORIGIN' + +ifeq ($(OSTYPE),linux) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libdriver.so $(WEBOTS_CONTROLLER_LIB_PATH)/libCppController.so +endif + +ifeq ($(OSTYPE),darwin) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/libdriver.dylib $(WEBOTS_CONTROLLER_LIB_PATH)/libCppController.dylib +endif + +ifeq ($(OSTYPE),windows) +LINK_DEPENDENCIES = $(WEBOTS_CONTROLLER_LIB_PATH)/driver.dll $(WEBOTS_CONTROLLER_LIB_PATH)/CppController.dll +endif + +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/src/Driver.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/src/Driver.cpp new file mode 100644 index 000000000..e79313157 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/cpp/driver/src/Driver.cpp @@ -0,0 +1,195 @@ +// Copyright 1996-2022 Cyberbotics Ltd. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Description: CPP wrapper of the driver library + * Comments: Sponsored by the CTI project RO2IVSim + * (http://transport.epfl.ch/simulator-for-mobile-robots-and-intelligent-vehicles) + */ + +#include +#include + +#include + +using namespace webots; + +Driver *Driver::dInstance = NULL; + +Driver::Driver() { + if (dInstance == NULL) { + dInstance = this; + wbu_driver_init(); + } else { + std::cerr << "Only one instance of the Driver class should be created" << std::endl; + exit(-1); + } +} + +Driver::~Driver() { + wbu_driver_cleanup(); +} + +bool Driver::isInitialisationPossible() { + return wbu_driver_initialization_is_possible(); +} + +Driver *Driver::getDriverInstance() { + if (dInstance) + return dInstance; + return new Driver(); +} + +int Driver::step() { + assert(this); + return wbu_driver_step(); +} + +void Driver::setSteeringAngle(double steeringAngle) { + assert(this); + wbu_driver_set_steering_angle(steeringAngle); +} + +double Driver::getSteeringAngle() { + assert(this); + return wbu_driver_get_steering_angle(); +} + +void Driver::setCruisingSpeed(double speed) { + assert(this); + wbu_driver_set_cruising_speed(speed); +} + +double Driver::getTargetCruisingSpeed() { + assert(this); + return wbu_driver_get_target_cruising_speed(); +} + +double Driver::getCurrentSpeed() { + assert(this); + return wbu_driver_get_current_speed(); +} + +void Driver::setThrottle(double throttle) { + assert(this); + wbu_driver_set_throttle(throttle); +} + +double Driver::getThrottle() { + assert(this); + return wbu_driver_get_throttle(); +} + +void Driver::setBrakeIntensity(double intensity) { + assert(this); + wbu_driver_set_brake_intensity(intensity); +} + +double Driver::getBrakeIntensity() { + assert(this); + return wbu_driver_get_brake_intensity(); +} + +void Driver::setBrake(double brake) { + assert(this); + std::cerr << "Warning: Deprecated 'setBrake' use 'setBrakeIntensity' instead." << std::endl; + setBrakeIntensity(brake); +} + +void Driver::setIndicator(IndicatorState state) { + assert(this); + wbu_driver_set_indicator(WbuDriverIndicatorState(state)); +} + +void Driver::setHazardFlashers(bool state) { + assert(this); + wbu_driver_set_hazard_flashers(state); +} + +Driver::IndicatorState Driver::getIndicator() { + assert(this); + return IndicatorState(wbu_driver_get_indicator()); +} + +bool Driver::getHazardFlashers() { + assert(this); + return wbu_driver_get_hazard_flashers(); +} + +void Driver::setDippedBeams(bool state) { + assert(this); + wbu_driver_set_dipped_beams(state); +} + +void Driver::setAntifogLights(bool state) { + assert(this); + wbu_driver_set_antifog_lights(state); +} + +bool Driver::getDippedBeams() { + assert(this); + return wbu_driver_get_dipped_beams(); +} + +bool Driver::getAntifogLights() { + assert(this); + return wbu_driver_get_antifog_lights(); +} + +double Driver::getRpm() { + assert(this); + return wbu_driver_get_rpm(); +} + +int Driver::getGear() { + assert(this); + return wbu_driver_get_gear(); +} + +void Driver::setGear(int gear) { + assert(this); + wbu_driver_set_gear(gear); +} + +int Driver::getGearNumber() { + assert(this); + return wbu_driver_get_gear_number(); +} + +Driver::ControlMode Driver::getControlMode() { + assert(this); + return ControlMode(wbu_driver_get_control_mode()); +} + +void Driver::setWiperMode(Driver::WiperMode mode) { + assert(this); + wbu_driver_set_wiper_mode(WbuDriverWiperMode(mode)); +} + +Driver::WiperMode Driver::getWiperMode() { + assert(this); + return WiperMode(wbu_driver_get_wiper_mode()); +} + +void Driver::setWipersMode(Driver::WiperMode mode) { + assert(this); + std::cerr << "Warning: Deprecated 'setWipersMode' use 'setWiperMode' instead." << std::endl; + wbu_driver_set_wiper_mode(WbuDriverWiperMode(mode)); +} + +Driver::WiperMode Driver::getWipersMode() { + assert(this); + std::cerr << "Warning: Deprecated 'getWipersMode' use 'getWiperMode' instead." << std::endl; + return WiperMode(wbu_driver_get_wiper_mode()); +} diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/.gitignore b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/.gitignore new file mode 100644 index 000000000..748e52f50 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/.gitignore @@ -0,0 +1,4 @@ +/vehicle.cpp +/output +/*jnilib + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/Makefile b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/Makefile new file mode 100644 index 000000000..95072f3e9 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/Makefile @@ -0,0 +1,114 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +WEBOTS_HOME_PATH_NO_ESCAPE=$(subst \,/,$(WEBOTS_HOME)) +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +WEBOTS_LIBRARY = 1 +MODULE_NAME = vehicle + +INTERFACE = $(MODULE_NAME).i +SOURCE_HEADERS = $(substr $(WEBOTS_HOME_PATH_NO_ESCAPE),$(WEBOTS_HOME_PATH),$(wildcard $(WEBOTS_HOME_PATH)/include/controller/cpp/webots/*.hpp)) +JAVA_FILES = Driver.java Car.java vehicle.java vehicleJNI.java + +WRAPPER = $(MODULE_NAME).cpp +WRAPPER_OBJECT = $(WRAPPER:.cpp=.o) +JAR = $(WEBOTS_CONTROLLER_LIB_PATH)/java/$(MODULE_NAME).jar +INCLUDES = -I$(WEBOTS_HOME_PATH)/include/controller/c/ -I$(WEBOTS_HOME_PATH)/include/controller/cpp/ + +SWIG_OUTPUT = output +MODULE_PATH = com/cyberbotics/webots/controller/vehicle +JAVA_SOURCES = $(addprefix $(SWIG_OUTPUT)/,$(JAVA_FILES)) + +SWIG_EXISTS = $(shell which swig 2> /dev/null) + +ifeq ($(OSTYPE),linux) +JAVA_INCLUDES = -I"$(JAVA_HOME)/include" -I"$(JAVA_HOME)/include/linux" +CFLAGS2 = -c -Wall -Wno-unused-function -fPIC +LIBRARY = $(WEBOTS_CONTROLLER_LIB_PATH)/java/libvehicle.so +endif + +ifeq ($(OSTYPE),windows) +JAVA_INCLUDES = -I"$(JAVA_HOME)/include" -I"$(JAVA_HOME)/include/win32" +CFLAGS2 = -c -Wall -Wno-unused-function -O +LIBRARY = $(WEBOTS_CONTROLLER_LIB_PATH)/java/vehicle.dll +endif + +ifeq ($(OSTYPE),darwin) +WRAPPER_OBJECT_ARM64 = $(WRAPPER:.cpp=-arm64.o) +WRAPPER_OBJECT_X86_64 = $(WRAPPER:.cpp=-x86_64.o) +JAVA_INCLUDES = -I"$(JAVA_HOME)/include" -I"$(JAVA_HOME)/include/darwin" -I"$(MACOSX_SDK_PATH)/System/Library/Frameworks/JavaVM.framework/Versions/Current/Headers" +CFLAGS1 = -isysroot $(MACOSX_SDK_PATH) -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -stdlib=libc++ -Wl,-rpath,@loader_path/../../.. +CFLAGS2 = -c -Wall -isysroot $(MACOSX_SDK_PATH) -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -stdlib=libc++ -Wno-unused-function +LIBRARY = $(WEBOTS_CONTROLLER_LIB_PATH)/java/libvehicle.jnilib +endif + +LIB += -L"$(WEBOTS_CONTROLLER_LIB_PATH)" -lCppController -lCppCar -lCppDriver + +.PHONY: JAVA_CLASSES all + +# rules +ifeq ($(SWIG_EXISTS),) +all release debug profile: + @$(ECHO) "# \033[0;33mSWIG not installed, skipping Java API\033[0m" +else ifeq ($(JAVA_HOME),) +all release debug profile: + @$(ECHO) "# \033[0;33mJava not installed or 'JAVA_HOME' not set, skipping Java API\033[0m" +else +all release debug profile: $(JAR) $(LIBRARY) + +$(JAR): JAVA_CLASSES + @mkdir -p $(WEBOTS_CONTROLLER_LIB_PATH)/java + @jar cfv "$@" $(MODULE_PATH)/*.class > /dev/null + +JAVA_CLASSES: $(JAVA_SOURCES) + @javac --release 8 -classpath $(WEBOTS_CONTROLLER_LIB_PATH)/java/Controller.jar $(JAVA_SOURCES) -d . + +$(JAVA_SOURCES): $(WRAPPER) + +$(WRAPPER): $(INTERFACE) $(SOURCE_HEADERS) + @mkdir -p $(SWIG_OUTPUT) + @swig -c++ -java -package $(subst /,.,$(MODULE_PATH)) -outdir $(SWIG_OUTPUT) -I$(WEBOTS_HOME_PATH)/src/controller/java/ $(INCLUDES) -o $@ $< + +$(WRAPPER_OBJECT): $(WRAPPER) + @$(CXX) $(CFLAGS2) $(INCLUDES) $(JAVA_INCLUDES) $< -o "$@" + +$(WRAPPER_OBJECT_ARM64): $(WRAPPER) + @$(CXX) -target arm64-apple-macos11 $(CFLAGS2) $(INCLUDES) $(JAVA_INCLUDES) $< -o "$@" + +$(WRAPPER_OBJECT_X86_64): $(WRAPPER) + @$(CXX) -target x86_64-apple-macos11 $(CFLAGS2) $(INCLUDES) $(JAVA_INCLUDES) $< -o "$@" + +%.so: $(WRAPPER_OBJECT) + @mkdir -p $(WEBOTS_CONTROLLER_LIB_PATH)/java + @$(CXX) -shared $< $(LIB) -o "$@" + +%.dll: $(WRAPPER_OBJECT) + @mkdir -p $(WEBOTS_CONTROLLER_LIB_PATH)/java + @$(CXX) -shared -mwindows -Wl,--add-stdcall-alias -Wl,--enable-auto-import -O -lm $< $(LIB) -o "$@" + +%.jnilib: $(WRAPPER_OBJECT_ARM64) $(WRAPPER_OBJECT_X86_64) + @$(CXX) -target arm64-apple-macos11 $(CFLAGS1) -bundle $(WRAPPER_OBJECT_ARM64) $(LIB) -o lib$(MODULE_NAME)-arm64.jnilib + @$(CXX) -target x86_64-apple-macos11 $(CFLAGS1) -bundle $(WRAPPER_OBJECT_X86_64) $(LIB) -o lib$(MODULE_NAME)-x86_64.jnilib + @mkdir -p $(WEBOTS_CONTROLLER_LIB_PATH)/java + @lipo -create -output "$@" lib$(MODULE_NAME)-arm64.jnilib lib$(MODULE_NAME)-x86_64.jnilib + @codesign -s - "$@" +endif + +clean: + @rm -fr com $(SWIG_OUTPUT) $(JAR) $(WRAPPER) $(LIBRARY) *.o *.jnilib + @rm -d $(WEBOTS_CONTROLLER_LIB_PATH)/java > /dev/null 2>&1 || : diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/vehicle.i b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/vehicle.i new file mode 100644 index 000000000..44d34b230 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/java/vehicle.i @@ -0,0 +1,29 @@ +%module vehicle + +%pragma(java) jniclasscode=%{ + static { + try { + System.loadLibrary("vehicle"); + } catch (UnsatisfiedLinkError e) { + System.err.println("Native code library failed to load. See the chapter on Dynamic Linking Problems in the SWIG Java documentation for help.\n" + e); + System.exit(1); + } + } +%} + +%include "enumsimple.swg" +%javaconst(1); + +%{ +#include +#include +%} + +%import "controller.i" + +%typemap(javaimports) SWIGTYPE %{ +import com.cyberbotics.webots.controller.Supervisor; +%} + +%include +%include diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle310.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle310.cpp new file mode 100644 index 000000000..cbfd22412 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle310.cpp @@ -0,0 +1,5255 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 4.0.2 + * + * This file is not intended to be easily readable and contains a number of + * coding conventions designed to improve portability and efficiency. Do not make + * changes to this file unless you know what you are doing--modify the SWIG + * interface file instead. + * ----------------------------------------------------------------------------- */ + + +#ifndef SWIGPYTHON +#define SWIGPYTHON +#endif + +#define SWIG_PYTHON_DIRECTOR_NO_VTABLE + + +#ifdef __cplusplus +/* SwigValueWrapper is described in swig.swg */ +template class SwigValueWrapper { + struct SwigMovePointer { + T *ptr; + SwigMovePointer(T *p) : ptr(p) { } + ~SwigMovePointer() { delete ptr; } + SwigMovePointer& operator=(SwigMovePointer& rhs) { T* oldptr = ptr; ptr = 0; delete oldptr; ptr = rhs.ptr; rhs.ptr = 0; return *this; } + } pointer; + SwigValueWrapper& operator=(const SwigValueWrapper& rhs); + SwigValueWrapper(const SwigValueWrapper& rhs); +public: + SwigValueWrapper() : pointer(0) { } + SwigValueWrapper& operator=(const T& t) { SwigMovePointer tmp(new T(t)); pointer = tmp; return *this; } + operator T&() const { return *pointer.ptr; } + T *operator&() { return pointer.ptr; } +}; + +template T SwigValueInit() { + return T(); +} +#endif + +/* ----------------------------------------------------------------------------- + * This section contains generic SWIG labels for method/variable + * declarations/attributes, and other compiler dependent labels. + * ----------------------------------------------------------------------------- */ + +/* template workaround for compilers that cannot correctly implement the C++ standard */ +#ifndef SWIGTEMPLATEDISAMBIGUATOR +# if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x560) +# define SWIGTEMPLATEDISAMBIGUATOR template +# elif defined(__HP_aCC) +/* Needed even with `aCC -AA' when `aCC -V' reports HP ANSI C++ B3910B A.03.55 */ +/* If we find a maximum version that requires this, the test would be __HP_aCC <= 35500 for A.03.55 */ +# define SWIGTEMPLATEDISAMBIGUATOR template +# else +# define SWIGTEMPLATEDISAMBIGUATOR +# endif +#endif + +/* inline attribute */ +#ifndef SWIGINLINE +# if defined(__cplusplus) || (defined(__GNUC__) && !defined(__STRICT_ANSI__)) +# define SWIGINLINE inline +# else +# define SWIGINLINE +# endif +#endif + +/* attribute recognised by some compilers to avoid 'unused' warnings */ +#ifndef SWIGUNUSED +# if defined(__GNUC__) +# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +# elif defined(__ICC) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +#endif + +#ifndef SWIG_MSC_UNSUPPRESS_4505 +# if defined(_MSC_VER) +# pragma warning(disable : 4505) /* unreferenced local function has been removed */ +# endif +#endif + +#ifndef SWIGUNUSEDPARM +# ifdef __cplusplus +# define SWIGUNUSEDPARM(p) +# else +# define SWIGUNUSEDPARM(p) p SWIGUNUSED +# endif +#endif + +/* internal SWIG method */ +#ifndef SWIGINTERN +# define SWIGINTERN static SWIGUNUSED +#endif + +/* internal inline SWIG method */ +#ifndef SWIGINTERNINLINE +# define SWIGINTERNINLINE SWIGINTERN SWIGINLINE +#endif + +/* exporting methods */ +#if defined(__GNUC__) +# if (__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) +# ifndef GCC_HASCLASSVISIBILITY +# define GCC_HASCLASSVISIBILITY +# endif +# endif +#endif + +#ifndef SWIGEXPORT +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# if defined(STATIC_LINKED) +# define SWIGEXPORT +# else +# define SWIGEXPORT __declspec(dllexport) +# endif +# else +# if defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) +# define SWIGEXPORT __attribute__ ((visibility("default"))) +# else +# define SWIGEXPORT +# endif +# endif +#endif + +/* calling conventions for Windows */ +#ifndef SWIGSTDCALL +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# define SWIGSTDCALL __stdcall +# else +# define SWIGSTDCALL +# endif +#endif + +/* Deal with Microsoft's attempt at deprecating C standard runtime functions */ +#if !defined(SWIG_NO_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_CRT_SECURE_NO_DEPRECATE) +# define _CRT_SECURE_NO_DEPRECATE +#endif + +/* Deal with Microsoft's attempt at deprecating methods in the standard C++ library */ +#if !defined(SWIG_NO_SCL_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_SCL_SECURE_NO_DEPRECATE) +# define _SCL_SECURE_NO_DEPRECATE +#endif + +/* Deal with Apple's deprecated 'AssertMacros.h' from Carbon-framework */ +#if defined(__APPLE__) && !defined(__ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES) +# define __ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES 0 +#endif + +/* Intel's compiler complains if a variable which was never initialised is + * cast to void, which is a common idiom which we use to indicate that we + * are aware a variable isn't used. So we just silence that warning. + * See: https://github.com/swig/swig/issues/192 for more discussion. + */ +#ifdef __INTEL_COMPILER +# pragma warning disable 592 +#endif + + +#if defined(__GNUC__) && defined(_WIN32) && !defined(SWIG_PYTHON_NO_HYPOT_WORKAROUND) +/* Workaround for '::hypot' has not been declared', see https://bugs.python.org/issue11566 */ +# include +#endif + +#if defined(_DEBUG) && defined(SWIG_PYTHON_INTERPRETER_NO_DEBUG) +/* Use debug wrappers with the Python release dll */ +# undef _DEBUG +# include +# define _DEBUG 1 +#else +# include +#endif + +/* ----------------------------------------------------------------------------- + * swigrun.swg + * + * This file contains generic C API SWIG runtime support for pointer + * type checking. + * ----------------------------------------------------------------------------- */ + +/* This should only be incremented when either the layout of swig_type_info changes, + or for whatever reason, the runtime changes incompatibly */ +#define SWIG_RUNTIME_VERSION "4" + +/* define SWIG_TYPE_TABLE_NAME as "SWIG_TYPE_TABLE" */ +#ifdef SWIG_TYPE_TABLE +# define SWIG_QUOTE_STRING(x) #x +# define SWIG_EXPAND_AND_QUOTE_STRING(x) SWIG_QUOTE_STRING(x) +# define SWIG_TYPE_TABLE_NAME SWIG_EXPAND_AND_QUOTE_STRING(SWIG_TYPE_TABLE) +#else +# define SWIG_TYPE_TABLE_NAME +#endif + +/* + You can use the SWIGRUNTIME and SWIGRUNTIMEINLINE macros for + creating a static or dynamic library from the SWIG runtime code. + In 99.9% of the cases, SWIG just needs to declare them as 'static'. + + But only do this if strictly necessary, ie, if you have problems + with your compiler or suchlike. +*/ + +#ifndef SWIGRUNTIME +# define SWIGRUNTIME SWIGINTERN +#endif + +#ifndef SWIGRUNTIMEINLINE +# define SWIGRUNTIMEINLINE SWIGRUNTIME SWIGINLINE +#endif + +/* Generic buffer size */ +#ifndef SWIG_BUFFER_SIZE +# define SWIG_BUFFER_SIZE 1024 +#endif + +/* Flags for pointer conversions */ +#define SWIG_POINTER_DISOWN 0x1 +#define SWIG_CAST_NEW_MEMORY 0x2 +#define SWIG_POINTER_NO_NULL 0x4 + +/* Flags for new pointer objects */ +#define SWIG_POINTER_OWN 0x1 + + +/* + Flags/methods for returning states. + + The SWIG conversion methods, as ConvertPtr, return an integer + that tells if the conversion was successful or not. And if not, + an error code can be returned (see swigerrors.swg for the codes). + + Use the following macros/flags to set or process the returning + states. + + In old versions of SWIG, code such as the following was usually written: + + if (SWIG_ConvertPtr(obj,vptr,ty.flags) != -1) { + // success code + } else { + //fail code + } + + Now you can be more explicit: + + int res = SWIG_ConvertPtr(obj,vptr,ty.flags); + if (SWIG_IsOK(res)) { + // success code + } else { + // fail code + } + + which is the same really, but now you can also do + + Type *ptr; + int res = SWIG_ConvertPtr(obj,(void **)(&ptr),ty.flags); + if (SWIG_IsOK(res)) { + // success code + if (SWIG_IsNewObj(res) { + ... + delete *ptr; + } else { + ... + } + } else { + // fail code + } + + I.e., now SWIG_ConvertPtr can return new objects and you can + identify the case and take care of the deallocation. Of course that + also requires SWIG_ConvertPtr to return new result values, such as + + int SWIG_ConvertPtr(obj, ptr,...) { + if () { + if () { + *ptr = ; + return SWIG_NEWOBJ; + } else { + *ptr = ; + return SWIG_OLDOBJ; + } + } else { + return SWIG_BADOBJ; + } + } + + Of course, returning the plain '0(success)/-1(fail)' still works, but you can be + more explicit by returning SWIG_BADOBJ, SWIG_ERROR or any of the + SWIG errors code. + + Finally, if the SWIG_CASTRANK_MODE is enabled, the result code + allows to return the 'cast rank', for example, if you have this + + int food(double) + int fooi(int); + + and you call + + food(1) // cast rank '1' (1 -> 1.0) + fooi(1) // cast rank '0' + + just use the SWIG_AddCast()/SWIG_CheckState() +*/ + +#define SWIG_OK (0) +#define SWIG_ERROR (-1) +#define SWIG_IsOK(r) (r >= 0) +#define SWIG_ArgError(r) ((r != SWIG_ERROR) ? r : SWIG_TypeError) + +/* The CastRankLimit says how many bits are used for the cast rank */ +#define SWIG_CASTRANKLIMIT (1 << 8) +/* The NewMask denotes the object was created (using new/malloc) */ +#define SWIG_NEWOBJMASK (SWIG_CASTRANKLIMIT << 1) +/* The TmpMask is for in/out typemaps that use temporal objects */ +#define SWIG_TMPOBJMASK (SWIG_NEWOBJMASK << 1) +/* Simple returning values */ +#define SWIG_BADOBJ (SWIG_ERROR) +#define SWIG_OLDOBJ (SWIG_OK) +#define SWIG_NEWOBJ (SWIG_OK | SWIG_NEWOBJMASK) +#define SWIG_TMPOBJ (SWIG_OK | SWIG_TMPOBJMASK) +/* Check, add and del mask methods */ +#define SWIG_AddNewMask(r) (SWIG_IsOK(r) ? (r | SWIG_NEWOBJMASK) : r) +#define SWIG_DelNewMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_NEWOBJMASK) : r) +#define SWIG_IsNewObj(r) (SWIG_IsOK(r) && (r & SWIG_NEWOBJMASK)) +#define SWIG_AddTmpMask(r) (SWIG_IsOK(r) ? (r | SWIG_TMPOBJMASK) : r) +#define SWIG_DelTmpMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_TMPOBJMASK) : r) +#define SWIG_IsTmpObj(r) (SWIG_IsOK(r) && (r & SWIG_TMPOBJMASK)) + +/* Cast-Rank Mode */ +#if defined(SWIG_CASTRANK_MODE) +# ifndef SWIG_TypeRank +# define SWIG_TypeRank unsigned long +# endif +# ifndef SWIG_MAXCASTRANK /* Default cast allowed */ +# define SWIG_MAXCASTRANK (2) +# endif +# define SWIG_CASTRANKMASK ((SWIG_CASTRANKLIMIT) -1) +# define SWIG_CastRank(r) (r & SWIG_CASTRANKMASK) +SWIGINTERNINLINE int SWIG_AddCast(int r) { + return SWIG_IsOK(r) ? ((SWIG_CastRank(r) < SWIG_MAXCASTRANK) ? (r + 1) : SWIG_ERROR) : r; +} +SWIGINTERNINLINE int SWIG_CheckState(int r) { + return SWIG_IsOK(r) ? SWIG_CastRank(r) + 1 : 0; +} +#else /* no cast-rank mode */ +# define SWIG_AddCast(r) (r) +# define SWIG_CheckState(r) (SWIG_IsOK(r) ? 1 : 0) +#endif + + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void *(*swig_converter_func)(void *, int *); +typedef struct swig_type_info *(*swig_dycast_func)(void **); + +/* Structure to store information on one type */ +typedef struct swig_type_info { + const char *name; /* mangled name of this type */ + const char *str; /* human readable name of this type */ + swig_dycast_func dcast; /* dynamic cast function down a hierarchy */ + struct swig_cast_info *cast; /* linked list of types that can cast into this type */ + void *clientdata; /* language specific type data */ + int owndata; /* flag if the structure owns the clientdata */ +} swig_type_info; + +/* Structure to store a type and conversion function used for casting */ +typedef struct swig_cast_info { + swig_type_info *type; /* pointer to type that is equivalent to this type */ + swig_converter_func converter; /* function to cast the void pointers */ + struct swig_cast_info *next; /* pointer to next cast in linked list */ + struct swig_cast_info *prev; /* pointer to the previous cast */ +} swig_cast_info; + +/* Structure used to store module information + * Each module generates one structure like this, and the runtime collects + * all of these structures and stores them in a circularly linked list.*/ +typedef struct swig_module_info { + swig_type_info **types; /* Array of pointers to swig_type_info structures that are in this module */ + size_t size; /* Number of types in this module */ + struct swig_module_info *next; /* Pointer to next element in circularly linked list */ + swig_type_info **type_initial; /* Array of initially generated type structures */ + swig_cast_info **cast_initial; /* Array of initially generated casting structures */ + void *clientdata; /* Language specific module data */ +} swig_module_info; + +/* + Compare two type names skipping the space characters, therefore + "char*" == "char *" and "Class" == "Class", etc. + + Return 0 when the two name types are equivalent, as in + strncmp, but skipping ' '. +*/ +SWIGRUNTIME int +SWIG_TypeNameComp(const char *f1, const char *l1, + const char *f2, const char *l2) { + for (;(f1 != l1) && (f2 != l2); ++f1, ++f2) { + while ((*f1 == ' ') && (f1 != l1)) ++f1; + while ((*f2 == ' ') && (f2 != l2)) ++f2; + if (*f1 != *f2) return (*f1 > *f2) ? 1 : -1; + } + return (int)((l1 - f1) - (l2 - f2)); +} + +/* + Check type equivalence in a name list like ||... + Return 0 if equal, -1 if nb < tb, 1 if nb > tb +*/ +SWIGRUNTIME int +SWIG_TypeCmp(const char *nb, const char *tb) { + int equiv = 1; + const char* te = tb + strlen(tb); + const char* ne = nb; + while (equiv != 0 && *ne) { + for (nb = ne; *ne; ++ne) { + if (*ne == '|') break; + } + equiv = SWIG_TypeNameComp(nb, ne, tb, te); + if (*ne) ++ne; + } + return equiv; +} + +/* + Check type equivalence in a name list like ||... + Return 0 if not equal, 1 if equal +*/ +SWIGRUNTIME int +SWIG_TypeEquiv(const char *nb, const char *tb) { + return SWIG_TypeCmp(nb, tb) == 0 ? 1 : 0; +} + +/* + Check the typename +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheck(const char *c, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (strcmp(iter->type->name, c) == 0) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Identical to SWIG_TypeCheck, except strcmp is replaced with a pointer comparison +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheckStruct(swig_type_info *from, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (iter->type == from) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Cast a pointer up an inheritance hierarchy +*/ +SWIGRUNTIMEINLINE void * +SWIG_TypeCast(swig_cast_info *ty, void *ptr, int *newmemory) { + return ((!ty) || (!ty->converter)) ? ptr : (*ty->converter)(ptr, newmemory); +} + +/* + Dynamic pointer casting. Down an inheritance hierarchy +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeDynamicCast(swig_type_info *ty, void **ptr) { + swig_type_info *lastty = ty; + if (!ty || !ty->dcast) return ty; + while (ty && (ty->dcast)) { + ty = (*ty->dcast)(ptr); + if (ty) lastty = ty; + } + return lastty; +} + +/* + Return the name associated with this type +*/ +SWIGRUNTIMEINLINE const char * +SWIG_TypeName(const swig_type_info *ty) { + return ty->name; +} + +/* + Return the pretty name associated with this type, + that is an unmangled type name in a form presentable to the user. +*/ +SWIGRUNTIME const char * +SWIG_TypePrettyName(const swig_type_info *type) { + /* The "str" field contains the equivalent pretty names of the + type, separated by vertical-bar characters. We choose + to print the last name, as it is often (?) the most + specific. */ + if (!type) return NULL; + if (type->str != NULL) { + const char *last_name = type->str; + const char *s; + for (s = type->str; *s; s++) + if (*s == '|') last_name = s+1; + return last_name; + } + else + return type->name; +} + +/* + Set the clientdata field for a type +*/ +SWIGRUNTIME void +SWIG_TypeClientData(swig_type_info *ti, void *clientdata) { + swig_cast_info *cast = ti->cast; + /* if (ti->clientdata == clientdata) return; */ + ti->clientdata = clientdata; + + while (cast) { + if (!cast->converter) { + swig_type_info *tc = cast->type; + if (!tc->clientdata) { + SWIG_TypeClientData(tc, clientdata); + } + } + cast = cast->next; + } +} +SWIGRUNTIME void +SWIG_TypeNewClientData(swig_type_info *ti, void *clientdata) { + SWIG_TypeClientData(ti, clientdata); + ti->owndata = 1; +} + +/* + Search for a swig_type_info structure only by mangled name + Search is a O(log #types) + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_MangledTypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + swig_module_info *iter = start; + do { + if (iter->size) { + size_t l = 0; + size_t r = iter->size - 1; + do { + /* since l+r >= 0, we can (>> 1) instead (/ 2) */ + size_t i = (l + r) >> 1; + const char *iname = iter->types[i]->name; + if (iname) { + int compare = strcmp(name, iname); + if (compare == 0) { + return iter->types[i]; + } else if (compare < 0) { + if (i) { + r = i - 1; + } else { + break; + } + } else if (compare > 0) { + l = i + 1; + } + } else { + break; /* should never happen */ + } + } while (l <= r); + } + iter = iter->next; + } while (iter != end); + return 0; +} + +/* + Search for a swig_type_info structure for either a mangled name or a human readable name. + It first searches the mangled names of the types, which is a O(log #types) + If a type is not found it then searches the human readable names, which is O(#types). + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + /* STEP 1: Search the name field using binary search */ + swig_type_info *ret = SWIG_MangledTypeQueryModule(start, end, name); + if (ret) { + return ret; + } else { + /* STEP 2: If the type hasn't been found, do a complete search + of the str field (the human readable name) */ + swig_module_info *iter = start; + do { + size_t i = 0; + for (; i < iter->size; ++i) { + if (iter->types[i]->str && (SWIG_TypeEquiv(iter->types[i]->str, name))) + return iter->types[i]; + } + iter = iter->next; + } while (iter != end); + } + + /* neither found a match */ + return 0; +} + +/* + Pack binary data into a string +*/ +SWIGRUNTIME char * +SWIG_PackData(char *c, void *ptr, size_t sz) { + static const char hex[17] = "0123456789abcdef"; + const unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + unsigned char uu = *u; + *(c++) = hex[(uu & 0xf0) >> 4]; + *(c++) = hex[uu & 0xf]; + } + return c; +} + +/* + Unpack binary data from a string +*/ +SWIGRUNTIME const char * +SWIG_UnpackData(const char *c, void *ptr, size_t sz) { + unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + char d = *(c++); + unsigned char uu; + if ((d >= '0') && (d <= '9')) + uu = (unsigned char)((d - '0') << 4); + else if ((d >= 'a') && (d <= 'f')) + uu = (unsigned char)((d - ('a'-10)) << 4); + else + return (char *) 0; + d = *(c++); + if ((d >= '0') && (d <= '9')) + uu |= (unsigned char)(d - '0'); + else if ((d >= 'a') && (d <= 'f')) + uu |= (unsigned char)(d - ('a'-10)); + else + return (char *) 0; + *u = uu; + } + return c; +} + +/* + Pack 'void *' into a string buffer. +*/ +SWIGRUNTIME char * +SWIG_PackVoidPtr(char *buff, void *ptr, const char *name, size_t bsz) { + char *r = buff; + if ((2*sizeof(void *) + 2) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,&ptr,sizeof(void *)); + if (strlen(name) + 1 > (bsz - (r - buff))) return 0; + strcpy(r,name); + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackVoidPtr(const char *c, void **ptr, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + *ptr = (void *) 0; + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sizeof(void *)); +} + +SWIGRUNTIME char * +SWIG_PackDataName(char *buff, void *ptr, size_t sz, const char *name, size_t bsz) { + char *r = buff; + size_t lname = (name ? strlen(name) : 0); + if ((2*sz + 2 + lname) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,ptr,sz); + if (lname) { + strncpy(r,name,lname+1); + } else { + *r = 0; + } + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackDataName(const char *c, void *ptr, size_t sz, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + memset(ptr,0,sz); + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sz); +} + +#ifdef __cplusplus +} +#endif + +/* Errors in SWIG */ +#define SWIG_UnknownError -1 +#define SWIG_IOError -2 +#define SWIG_RuntimeError -3 +#define SWIG_IndexError -4 +#define SWIG_TypeError -5 +#define SWIG_DivisionByZero -6 +#define SWIG_OverflowError -7 +#define SWIG_SyntaxError -8 +#define SWIG_ValueError -9 +#define SWIG_SystemError -10 +#define SWIG_AttributeError -11 +#define SWIG_MemoryError -12 +#define SWIG_NullReferenceError -13 + + + +/* Compatibility macros for Python 3 */ +#if PY_VERSION_HEX >= 0x03000000 + +#define PyClass_Check(obj) PyObject_IsInstance(obj, (PyObject *)&PyType_Type) +#define PyInt_Check(x) PyLong_Check(x) +#define PyInt_AsLong(x) PyLong_AsLong(x) +#define PyInt_FromLong(x) PyLong_FromLong(x) +#define PyInt_FromSize_t(x) PyLong_FromSize_t(x) +#define PyString_Check(name) PyBytes_Check(name) +#define PyString_FromString(x) PyUnicode_FromString(x) +#define PyString_Format(fmt, args) PyUnicode_Format(fmt, args) +#define PyString_AsString(str) PyBytes_AsString(str) +#define PyString_Size(str) PyBytes_Size(str) +#define PyString_InternFromString(key) PyUnicode_InternFromString(key) +#define Py_TPFLAGS_HAVE_CLASS Py_TPFLAGS_BASETYPE +#define PyString_AS_STRING(x) PyUnicode_AS_STRING(x) +#define _PyLong_FromSsize_t(x) PyLong_FromSsize_t(x) + +#endif + +#ifndef Py_TYPE +# define Py_TYPE(op) ((op)->ob_type) +#endif + +/* SWIG APIs for compatibility of both Python 2 & 3 */ + +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_Python_str_FromFormat PyUnicode_FromFormat +#else +# define SWIG_Python_str_FromFormat PyString_FromFormat +#endif + + +/* Warning: This function will allocate a new string in Python 3, + * so please call SWIG_Python_str_DelForPy3(x) to free the space. + */ +SWIGINTERN char* +SWIG_Python_str_AsChar(PyObject *str) +{ +#if PY_VERSION_HEX >= 0x03030000 + return (char *)PyUnicode_AsUTF8(str); +#elif PY_VERSION_HEX >= 0x03000000 + char *newstr = 0; + str = PyUnicode_AsUTF8String(str); + if (str) { + char *cstr; + Py_ssize_t len; + if (PyBytes_AsStringAndSize(str, &cstr, &len) != -1) { + newstr = (char *) malloc(len+1); + if (newstr) + memcpy(newstr, cstr, len+1); + } + Py_XDECREF(str); + } + return newstr; +#else + return PyString_AsString(str); +#endif +} + +#if PY_VERSION_HEX >= 0x03030000 || PY_VERSION_HEX < 0x03000000 +# define SWIG_Python_str_DelForPy3(x) +#else +# define SWIG_Python_str_DelForPy3(x) free( (void*) (x) ) +#endif + + +SWIGINTERN PyObject* +SWIG_Python_str_FromChar(const char *c) +{ +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_FromString(c); +#else + return PyString_FromString(c); +#endif +} + +#ifndef PyObject_DEL +# define PyObject_DEL PyObject_Del +#endif + +// SWIGPY_USE_CAPSULE is no longer used within SWIG itself, but some user +// interface files check for it. +# define SWIGPY_USE_CAPSULE +# define SWIGPY_CAPSULE_NAME ("swig_runtime_data" SWIG_RUNTIME_VERSION ".type_pointer_capsule" SWIG_TYPE_TABLE_NAME) + +#if PY_VERSION_HEX < 0x03020000 +#define PyDescr_TYPE(x) (((PyDescrObject *)(x))->d_type) +#define PyDescr_NAME(x) (((PyDescrObject *)(x))->d_name) +#define Py_hash_t long +#endif + +/* ----------------------------------------------------------------------------- + * error manipulation + * ----------------------------------------------------------------------------- */ + +SWIGRUNTIME PyObject* +SWIG_Python_ErrorType(int code) { + PyObject* type = 0; + switch(code) { + case SWIG_MemoryError: + type = PyExc_MemoryError; + break; + case SWIG_IOError: + type = PyExc_IOError; + break; + case SWIG_RuntimeError: + type = PyExc_RuntimeError; + break; + case SWIG_IndexError: + type = PyExc_IndexError; + break; + case SWIG_TypeError: + type = PyExc_TypeError; + break; + case SWIG_DivisionByZero: + type = PyExc_ZeroDivisionError; + break; + case SWIG_OverflowError: + type = PyExc_OverflowError; + break; + case SWIG_SyntaxError: + type = PyExc_SyntaxError; + break; + case SWIG_ValueError: + type = PyExc_ValueError; + break; + case SWIG_SystemError: + type = PyExc_SystemError; + break; + case SWIG_AttributeError: + type = PyExc_AttributeError; + break; + default: + type = PyExc_RuntimeError; + } + return type; +} + + +SWIGRUNTIME void +SWIG_Python_AddErrorMsg(const char* mesg) +{ + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + + if (PyErr_Occurred()) + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + PyErr_Clear(); + Py_XINCREF(type); + if (tmp) + PyErr_Format(type, "%s %s", tmp, mesg); + else + PyErr_Format(type, "%s", mesg); + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + Py_DECREF(value); + } else { + PyErr_SetString(PyExc_RuntimeError, mesg); + } +} + +SWIGRUNTIME int +SWIG_Python_TypeErrorOccurred(PyObject *obj) +{ + PyObject *error; + if (obj) + return 0; + error = PyErr_Occurred(); + return error && PyErr_GivenExceptionMatches(error, PyExc_TypeError); +} + +SWIGRUNTIME void +SWIG_Python_RaiseOrModifyTypeError(const char *message) +{ + if (SWIG_Python_TypeErrorOccurred(NULL)) { + /* Use existing TypeError to preserve stacktrace and enhance with given message */ + PyObject *newvalue; + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); +#if PY_VERSION_HEX >= 0x03000000 + newvalue = PyUnicode_FromFormat("%S\nAdditional information:\n%s", value, message); +#else + newvalue = PyString_FromFormat("%s\nAdditional information:\n%s", PyString_AsString(value), message); +#endif + Py_XDECREF(value); + PyErr_Restore(type, newvalue, traceback); + } else { + /* Raise TypeError using given message */ + PyErr_SetString(PyExc_TypeError, message); + } +} + +#if defined(SWIG_PYTHON_NO_THREADS) +# if defined(SWIG_PYTHON_THREADS) +# undef SWIG_PYTHON_THREADS +# endif +#endif +#if defined(SWIG_PYTHON_THREADS) /* Threading support is enabled */ +# if !defined(SWIG_PYTHON_USE_GIL) && !defined(SWIG_PYTHON_NO_USE_GIL) +# define SWIG_PYTHON_USE_GIL +# endif +# if defined(SWIG_PYTHON_USE_GIL) /* Use PyGILState threads calls */ +# ifndef SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_INITIALIZE_THREADS PyEval_InitThreads() +# endif +# ifdef __cplusplus /* C++ code */ + class SWIG_Python_Thread_Block { + bool status; + PyGILState_STATE state; + public: + void end() { if (status) { PyGILState_Release(state); status = false;} } + SWIG_Python_Thread_Block() : status(true), state(PyGILState_Ensure()) {} + ~SWIG_Python_Thread_Block() { end(); } + }; + class SWIG_Python_Thread_Allow { + bool status; + PyThreadState *save; + public: + void end() { if (status) { PyEval_RestoreThread(save); status = false; }} + SWIG_Python_Thread_Allow() : status(true), save(PyEval_SaveThread()) {} + ~SWIG_Python_Thread_Allow() { end(); } + }; +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK SWIG_Python_Thread_Block _swig_thread_block +# define SWIG_PYTHON_THREAD_END_BLOCK _swig_thread_block.end() +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW SWIG_Python_Thread_Allow _swig_thread_allow +# define SWIG_PYTHON_THREAD_END_ALLOW _swig_thread_allow.end() +# else /* C code */ +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK PyGILState_STATE _swig_thread_block = PyGILState_Ensure() +# define SWIG_PYTHON_THREAD_END_BLOCK PyGILState_Release(_swig_thread_block) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW PyThreadState *_swig_thread_allow = PyEval_SaveThread() +# define SWIG_PYTHON_THREAD_END_ALLOW PyEval_RestoreThread(_swig_thread_allow) +# endif +# else /* Old thread way, not implemented, user must provide it */ +# if !defined(SWIG_PYTHON_INITIALIZE_THREADS) +# define SWIG_PYTHON_INITIALIZE_THREADS +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_BLOCK) +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_END_BLOCK) +# define SWIG_PYTHON_THREAD_END_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_ALLOW) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# endif +# if !defined(SWIG_PYTHON_THREAD_END_ALLOW) +# define SWIG_PYTHON_THREAD_END_ALLOW +# endif +# endif +#else /* No thread support */ +# define SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# define SWIG_PYTHON_THREAD_END_BLOCK +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# define SWIG_PYTHON_THREAD_END_ALLOW +#endif + +/* ----------------------------------------------------------------------------- + * Python API portion that goes into the runtime + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* ----------------------------------------------------------------------------- + * Constant declarations + * ----------------------------------------------------------------------------- */ + +/* Constant Types */ +#define SWIG_PY_POINTER 4 +#define SWIG_PY_BINARY 5 + +/* Constant information structure */ +typedef struct swig_const_info { + int type; + const char *name; + long lvalue; + double dvalue; + void *pvalue; + swig_type_info **ptype; +} swig_const_info; + +#ifdef __cplusplus +} +#endif + + +/* ----------------------------------------------------------------------------- + * pyrun.swg + * + * This file contains the runtime support for Python modules + * and includes code for managing global variables and pointer + * type checking. + * + * ----------------------------------------------------------------------------- */ + +#if PY_VERSION_HEX < 0x02070000 /* 2.7.0 */ +# error "This version of SWIG only supports Python >= 2.7" +#endif + +#if PY_VERSION_HEX >= 0x03000000 && PY_VERSION_HEX < 0x03020000 +# error "This version of SWIG only supports Python 3 >= 3.2" +#endif + +/* Common SWIG API */ + +/* for raw pointers */ +#define SWIG_Python_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, 0) +#define SWIG_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtr(obj, pptr, type, flags) +#define SWIG_ConvertPtrAndOwn(obj,pptr,type,flags,own) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, own) + +#ifdef SWIGPYTHON_BUILTIN +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(self, ptr, type, flags) +#else +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) +#endif + +#define SWIG_InternalNewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) + +#define SWIG_CheckImplicit(ty) SWIG_Python_CheckImplicit(ty) +#define SWIG_AcquirePtr(ptr, src) SWIG_Python_AcquirePtr(ptr, src) +#define swig_owntype int + +/* for raw packed data */ +#define SWIG_ConvertPacked(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewPackedObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + +/* for class or struct pointers */ +#define SWIG_ConvertInstance(obj, pptr, type, flags) SWIG_ConvertPtr(obj, pptr, type, flags) +#define SWIG_NewInstanceObj(ptr, type, flags) SWIG_NewPointerObj(ptr, type, flags) + +/* for C or C++ function pointers */ +#define SWIG_ConvertFunctionPtr(obj, pptr, type) SWIG_Python_ConvertFunctionPtr(obj, pptr, type) +#define SWIG_NewFunctionPtrObj(ptr, type) SWIG_Python_NewPointerObj(NULL, ptr, type, 0) + +/* for C++ member pointers, ie, member methods */ +#define SWIG_ConvertMember(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewMemberObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + + +/* Runtime API */ + +#define SWIG_GetModule(clientdata) SWIG_Python_GetModule(clientdata) +#define SWIG_SetModule(clientdata, pointer) SWIG_Python_SetModule(pointer) +#define SWIG_NewClientData(obj) SwigPyClientData_New(obj) + +#define SWIG_SetErrorObj SWIG_Python_SetErrorObj +#define SWIG_SetErrorMsg SWIG_Python_SetErrorMsg +#define SWIG_ErrorType(code) SWIG_Python_ErrorType(code) +#define SWIG_Error(code, msg) SWIG_Python_SetErrorMsg(SWIG_ErrorType(code), msg) +#define SWIG_fail goto fail + + +/* Runtime API implementation */ + +/* Error manipulation */ + +SWIGINTERN void +SWIG_Python_SetErrorObj(PyObject *errtype, PyObject *obj) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetObject(errtype, obj); + Py_DECREF(obj); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +SWIGINTERN void +SWIG_Python_SetErrorMsg(PyObject *errtype, const char *msg) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetString(errtype, msg); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +#define SWIG_Python_Raise(obj, type, desc) SWIG_Python_SetErrorObj(SWIG_Python_ExceptionType(desc), obj) + +/* Set a constant value */ + +#if defined(SWIGPYTHON_BUILTIN) + +SWIGINTERN void +SwigPyBuiltin_AddPublicSymbol(PyObject *seq, const char *key) { + PyObject *s = PyString_InternFromString(key); + PyList_Append(seq, s); + Py_DECREF(s); +} + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, PyObject *public_interface, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); + if (public_interface) + SwigPyBuiltin_AddPublicSymbol(public_interface, name); +} + +#else + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); +} + +#endif + +/* Append a value to the result obj */ + +SWIGINTERN PyObject* +SWIG_Python_AppendOutput(PyObject* result, PyObject* obj) { + if (!result) { + result = obj; + } else if (result == Py_None) { + Py_DECREF(result); + result = obj; + } else { + if (!PyList_Check(result)) { + PyObject *o2 = result; + result = PyList_New(1); + PyList_SetItem(result, 0, o2); + } + PyList_Append(result,obj); + Py_DECREF(obj); + } + return result; +} + +/* Unpack the argument tuple */ + +SWIGINTERN Py_ssize_t +SWIG_Python_UnpackTuple(PyObject *args, const char *name, Py_ssize_t min, Py_ssize_t max, PyObject **objs) +{ + if (!args) { + if (!min && !max) { + return 1; + } else { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got none", + name, (min == max ? "" : "at least "), (int)min); + return 0; + } + } + if (!PyTuple_Check(args)) { + if (min <= 1 && max >= 1) { + Py_ssize_t i; + objs[0] = args; + for (i = 1; i < max; ++i) { + objs[i] = 0; + } + return 2; + } + PyErr_SetString(PyExc_SystemError, "UnpackTuple() argument list is not a tuple"); + return 0; + } else { + Py_ssize_t l = PyTuple_GET_SIZE(args); + if (l < min) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at least "), (int)min, (int)l); + return 0; + } else if (l > max) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at most "), (int)max, (int)l); + return 0; + } else { + Py_ssize_t i; + for (i = 0; i < l; ++i) { + objs[i] = PyTuple_GET_ITEM(args, i); + } + for (; l < max; ++l) { + objs[l] = 0; + } + return i + 1; + } + } +} + +SWIGINTERN int +SWIG_Python_CheckNoKeywords(PyObject *kwargs, const char *name) { + int no_kwargs = 1; + if (kwargs) { + assert(PyDict_Check(kwargs)); + if (PyDict_Size(kwargs) > 0) { + PyErr_Format(PyExc_TypeError, "%s() does not take keyword arguments", name); + no_kwargs = 0; + } + } + return no_kwargs; +} + +/* A functor is a function object with one single object argument */ +#define SWIG_Python_CallFunctor(functor, obj) PyObject_CallFunctionObjArgs(functor, obj, NULL); + +/* + Helper for static pointer initialization for both C and C++ code, for example + static PyObject *SWIG_STATIC_POINTER(MyVar) = NewSomething(...); +*/ +#ifdef __cplusplus +#define SWIG_STATIC_POINTER(var) var +#else +#define SWIG_STATIC_POINTER(var) var = 0; if (!var) var +#endif + +/* ----------------------------------------------------------------------------- + * Pointer declarations + * ----------------------------------------------------------------------------- */ + +/* Flags for new pointer objects */ +#define SWIG_POINTER_NOSHADOW (SWIG_POINTER_OWN << 1) +#define SWIG_POINTER_NEW (SWIG_POINTER_NOSHADOW | SWIG_POINTER_OWN) + +#define SWIG_POINTER_IMPLICIT_CONV (SWIG_POINTER_DISOWN << 1) + +#define SWIG_BUILTIN_TP_INIT (SWIG_POINTER_OWN << 2) +#define SWIG_BUILTIN_INIT (SWIG_BUILTIN_TP_INIT | SWIG_POINTER_OWN) + +#ifdef __cplusplus +extern "C" { +#endif + +/* The python void return value */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Py_Void(void) +{ + PyObject *none = Py_None; + Py_INCREF(none); + return none; +} + +/* SwigPyClientData */ + +typedef struct { + PyObject *klass; + PyObject *newraw; + PyObject *newargs; + PyObject *destroy; + int delargs; + int implicitconv; + PyTypeObject *pytype; +} SwigPyClientData; + +SWIGRUNTIMEINLINE int +SWIG_Python_CheckImplicit(swig_type_info *ty) +{ + SwigPyClientData *data = (SwigPyClientData *)ty->clientdata; + int fail = data ? data->implicitconv : 0; + if (fail) + PyErr_SetString(PyExc_TypeError, "Implicit conversion is prohibited for explicit constructors."); + return fail; +} + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_ExceptionType(swig_type_info *desc) { + SwigPyClientData *data = desc ? (SwigPyClientData *) desc->clientdata : 0; + PyObject *klass = data ? data->klass : 0; + return (klass ? klass : PyExc_RuntimeError); +} + + +SWIGRUNTIME SwigPyClientData * +SwigPyClientData_New(PyObject* obj) +{ + if (!obj) { + return 0; + } else { + SwigPyClientData *data = (SwigPyClientData *)malloc(sizeof(SwigPyClientData)); + /* the klass element */ + data->klass = obj; + Py_INCREF(data->klass); + /* the newraw method and newargs arguments used to create a new raw instance */ + if (PyClass_Check(obj)) { + data->newraw = 0; + data->newargs = obj; + Py_INCREF(obj); + } else { + data->newraw = PyObject_GetAttrString(data->klass, "__new__"); + if (data->newraw) { + Py_INCREF(data->newraw); + data->newargs = PyTuple_New(1); + PyTuple_SetItem(data->newargs, 0, obj); + } else { + data->newargs = obj; + } + Py_INCREF(data->newargs); + } + /* the destroy method, aka as the C++ delete method */ + data->destroy = PyObject_GetAttrString(data->klass, "__swig_destroy__"); + if (PyErr_Occurred()) { + PyErr_Clear(); + data->destroy = 0; + } + if (data->destroy) { + int flags; + Py_INCREF(data->destroy); + flags = PyCFunction_GET_FLAGS(data->destroy); + data->delargs = !(flags & (METH_O)); + } else { + data->delargs = 0; + } + data->implicitconv = 0; + data->pytype = 0; + return data; + } +} + +SWIGRUNTIME void +SwigPyClientData_Del(SwigPyClientData *data) { + Py_XDECREF(data->newraw); + Py_XDECREF(data->newargs); + Py_XDECREF(data->destroy); +} + +/* =============== SwigPyObject =====================*/ + +typedef struct { + PyObject_HEAD + void *ptr; + swig_type_info *ty; + int own; + PyObject *next; +#ifdef SWIGPYTHON_BUILTIN + PyObject *dict; +#endif +} SwigPyObject; + + +#ifdef SWIGPYTHON_BUILTIN + +SWIGRUNTIME PyObject * +SwigPyObject_get___dict__(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + + if (!sobj->dict) + sobj->dict = PyDict_New(); + + Py_INCREF(sobj->dict); + return sobj->dict; +} + +#endif + +SWIGRUNTIME PyObject * +SwigPyObject_long(SwigPyObject *v) +{ + return PyLong_FromVoidPtr(v->ptr); +} + +SWIGRUNTIME PyObject * +SwigPyObject_format(const char* fmt, SwigPyObject *v) +{ + PyObject *res = NULL; + PyObject *args = PyTuple_New(1); + if (args) { + if (PyTuple_SetItem(args, 0, SwigPyObject_long(v)) == 0) { + PyObject *ofmt = SWIG_Python_str_FromChar(fmt); + if (ofmt) { +#if PY_VERSION_HEX >= 0x03000000 + res = PyUnicode_Format(ofmt,args); +#else + res = PyString_Format(ofmt,args); +#endif + Py_DECREF(ofmt); + } + Py_DECREF(args); + } + } + return res; +} + +SWIGRUNTIME PyObject * +SwigPyObject_oct(SwigPyObject *v) +{ + return SwigPyObject_format("%o",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_hex(SwigPyObject *v) +{ + return SwigPyObject_format("%x",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_repr(SwigPyObject *v) +{ + const char *name = SWIG_TypePrettyName(v->ty); + PyObject *repr = SWIG_Python_str_FromFormat("", (name ? name : "unknown"), (void *)v); + if (v->next) { + PyObject *nrep = SwigPyObject_repr((SwigPyObject *)v->next); +# if PY_VERSION_HEX >= 0x03000000 + PyObject *joined = PyUnicode_Concat(repr, nrep); + Py_DecRef(repr); + Py_DecRef(nrep); + repr = joined; +# else + PyString_ConcatAndDel(&repr,nrep); +# endif + } + return repr; +} + +/* We need a version taking two PyObject* parameters so it's a valid + * PyCFunction to use in swigobject_methods[]. */ +SWIGRUNTIME PyObject * +SwigPyObject_repr2(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + return SwigPyObject_repr((SwigPyObject*)v); +} + +SWIGRUNTIME int +SwigPyObject_compare(SwigPyObject *v, SwigPyObject *w) +{ + void *i = v->ptr; + void *j = w->ptr; + return (i < j) ? -1 : ((i > j) ? 1 : 0); +} + +/* Added for Python 3.x, would it also be useful for Python 2.x? */ +SWIGRUNTIME PyObject* +SwigPyObject_richcompare(SwigPyObject *v, SwigPyObject *w, int op) +{ + PyObject* res; + if( op != Py_EQ && op != Py_NE ) { + Py_INCREF(Py_NotImplemented); + return Py_NotImplemented; + } + res = PyBool_FromLong( (SwigPyObject_compare(v, w)==0) == (op == Py_EQ) ? 1 : 0); + return res; +} + + +SWIGRUNTIME PyTypeObject* SwigPyObject_TypeOnce(void); + +#ifdef SWIGPYTHON_BUILTIN +static swig_type_info *SwigPyObject_stype = 0; +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + SwigPyClientData *cd; + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + assert(cd); + assert(cd->pytype); + return cd->pytype; +} +#else +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyObject_TypeOnce(); + return type; +} +#endif + +SWIGRUNTIMEINLINE int +SwigPyObject_Check(PyObject *op) { +#ifdef SWIGPYTHON_BUILTIN + PyTypeObject *target_tp = SwigPyObject_type(); + if (PyType_IsSubtype(op->ob_type, target_tp)) + return 1; + return (strcmp(op->ob_type->tp_name, "SwigPyObject") == 0); +#else + return (Py_TYPE(op) == SwigPyObject_type()) + || (strcmp(Py_TYPE(op)->tp_name,"SwigPyObject") == 0); +#endif +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own); + +SWIGRUNTIME void +SwigPyObject_dealloc(PyObject *v) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + PyObject *next = sobj->next; + if (sobj->own == SWIG_POINTER_OWN) { + swig_type_info *ty = sobj->ty; + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + PyObject *destroy = data ? data->destroy : 0; + if (destroy) { + /* destroy is always a VARARGS method */ + PyObject *res; + + /* PyObject_CallFunction() has the potential to silently drop + the active exception. In cases of unnamed temporary + variable or where we just finished iterating over a generator + StopIteration will be active right now, and this needs to + remain true upon return from SwigPyObject_dealloc. So save + and restore. */ + + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); + + if (data->delargs) { + /* we need to create a temporary object to carry the destroy operation */ + PyObject *tmp = SwigPyObject_New(sobj->ptr, ty, 0); + res = SWIG_Python_CallFunctor(destroy, tmp); + Py_DECREF(tmp); + } else { + PyCFunction meth = PyCFunction_GET_FUNCTION(destroy); + PyObject *mself = PyCFunction_GET_SELF(destroy); + res = ((*meth)(mself, v)); + } + if (!res) + PyErr_WriteUnraisable(destroy); + + PyErr_Restore(type, value, traceback); + + Py_XDECREF(res); + } +#if !defined(SWIG_PYTHON_SILENT_MEMLEAK) + else { + const char *name = SWIG_TypePrettyName(ty); + printf("swig/python detected a memory leak of type '%s', no destructor found.\n", (name ? name : "unknown")); + } +#endif + } + Py_XDECREF(next); + PyObject_DEL(v); +} + +SWIGRUNTIME PyObject* +SwigPyObject_append(PyObject* v, PyObject* next) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (!SwigPyObject_Check(next)) { + PyErr_SetString(PyExc_TypeError, "Attempt to append a non SwigPyObject"); + return NULL; + } + sobj->next = next; + Py_INCREF(next); + return SWIG_Py_Void(); +} + +SWIGRUNTIME PyObject* +SwigPyObject_next(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (sobj->next) { + Py_INCREF(sobj->next); + return sobj->next; + } else { + return SWIG_Py_Void(); + } +} + +SWIGINTERN PyObject* +SwigPyObject_disown(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = 0; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_acquire(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = SWIG_POINTER_OWN; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_own(PyObject *v, PyObject *args) +{ + PyObject *val = 0; + if (!PyArg_UnpackTuple(args, "own", 0, 1, &val)) { + return NULL; + } else { + SwigPyObject *sobj = (SwigPyObject *)v; + PyObject *obj = PyBool_FromLong(sobj->own); + if (val) { + if (PyObject_IsTrue(val)) { + SwigPyObject_acquire(v,args); + } else { + SwigPyObject_disown(v,args); + } + } + return obj; + } +} + +static PyMethodDef +swigobject_methods[] = { + {"disown", SwigPyObject_disown, METH_NOARGS, "releases ownership of the pointer"}, + {"acquire", SwigPyObject_acquire, METH_NOARGS, "acquires ownership of the pointer"}, + {"own", SwigPyObject_own, METH_VARARGS, "returns/sets ownership of the pointer"}, + {"append", SwigPyObject_append, METH_O, "appends another 'this' object"}, + {"next", SwigPyObject_next, METH_NOARGS, "returns the next 'this' object"}, + {"__repr__",SwigPyObject_repr2, METH_NOARGS, "returns object representation"}, + {0, 0, 0, 0} +}; + +SWIGRUNTIME PyTypeObject* +SwigPyObject_TypeOnce(void) { + static char swigobject_doc[] = "Swig object carries a C/C++ instance pointer"; + + static PyNumberMethods SwigPyObject_as_number = { + (binaryfunc)0, /*nb_add*/ + (binaryfunc)0, /*nb_subtract*/ + (binaryfunc)0, /*nb_multiply*/ + /* nb_divide removed in Python 3 */ +#if PY_VERSION_HEX < 0x03000000 + (binaryfunc)0, /*nb_divide*/ +#endif + (binaryfunc)0, /*nb_remainder*/ + (binaryfunc)0, /*nb_divmod*/ + (ternaryfunc)0,/*nb_power*/ + (unaryfunc)0, /*nb_negative*/ + (unaryfunc)0, /*nb_positive*/ + (unaryfunc)0, /*nb_absolute*/ + (inquiry)0, /*nb_nonzero*/ + 0, /*nb_invert*/ + 0, /*nb_lshift*/ + 0, /*nb_rshift*/ + 0, /*nb_and*/ + 0, /*nb_xor*/ + 0, /*nb_or*/ +#if PY_VERSION_HEX < 0x03000000 + 0, /*nb_coerce*/ +#endif + (unaryfunc)SwigPyObject_long, /*nb_int*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_long, /*nb_long*/ +#else + 0, /*nb_reserved*/ +#endif + (unaryfunc)0, /*nb_float*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_oct, /*nb_oct*/ + (unaryfunc)SwigPyObject_hex, /*nb_hex*/ +#endif +#if PY_VERSION_HEX >= 0x03050000 /* 3.5 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_inplace_matrix_multiply */ +#elif PY_VERSION_HEX >= 0x03000000 /* 3.0 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index, nb_inplace_divide removed */ +#else + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index */ +#endif + }; + + static PyTypeObject swigpyobject_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyObject", /* tp_name */ + sizeof(SwigPyObject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyObject_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX >= 0x03000000 + 0, /* tp_reserved in 3.0.1, tp_compare in 3.0.0 but not used */ +#else + (cmpfunc)SwigPyObject_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyObject_repr, /* tp_repr */ + &SwigPyObject_as_number, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + 0, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigobject_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + (richcmpfunc)SwigPyObject_richcompare,/* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + swigobject_methods, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpyobject_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpyobject_type) < 0) + return NULL; + } + return &swigpyobject_type; +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own) +{ + SwigPyObject *sobj = PyObject_NEW(SwigPyObject, SwigPyObject_type()); + if (sobj) { + sobj->ptr = ptr; + sobj->ty = ty; + sobj->own = own; + sobj->next = 0; + } + return (PyObject *)sobj; +} + +/* ----------------------------------------------------------------------------- + * Implements a simple Swig Packed type, and use it instead of string + * ----------------------------------------------------------------------------- */ + +typedef struct { + PyObject_HEAD + void *pack; + swig_type_info *ty; + size_t size; +} SwigPyPacked; + +SWIGRUNTIME PyObject * +SwigPyPacked_repr(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))) { + return SWIG_Python_str_FromFormat("", result, v->ty->name); + } else { + return SWIG_Python_str_FromFormat("", v->ty->name); + } +} + +SWIGRUNTIME PyObject * +SwigPyPacked_str(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))){ + return SWIG_Python_str_FromFormat("%s%s", result, v->ty->name); + } else { + return SWIG_Python_str_FromChar(v->ty->name); + } +} + +SWIGRUNTIME int +SwigPyPacked_compare(SwigPyPacked *v, SwigPyPacked *w) +{ + size_t i = v->size; + size_t j = w->size; + int s = (i < j) ? -1 : ((i > j) ? 1 : 0); + return s ? s : strncmp((const char *)v->pack, (const char *)w->pack, 2*v->size); +} + +SWIGRUNTIME PyTypeObject* SwigPyPacked_TypeOnce(void); + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyPacked_TypeOnce(); + return type; +} + +SWIGRUNTIMEINLINE int +SwigPyPacked_Check(PyObject *op) { + return ((op)->ob_type == SwigPyPacked_TypeOnce()) + || (strcmp((op)->ob_type->tp_name,"SwigPyPacked") == 0); +} + +SWIGRUNTIME void +SwigPyPacked_dealloc(PyObject *v) +{ + if (SwigPyPacked_Check(v)) { + SwigPyPacked *sobj = (SwigPyPacked *) v; + free(sobj->pack); + } + PyObject_DEL(v); +} + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_TypeOnce(void) { + static char swigpacked_doc[] = "Swig object carries a C/C++ instance pointer"; + static PyTypeObject swigpypacked_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX>=0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyPacked", /* tp_name */ + sizeof(SwigPyPacked), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyPacked_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX>=0x03000000 + 0, /* tp_reserved in 3.0.1 */ +#else + (cmpfunc)SwigPyPacked_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyPacked_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + (reprfunc)SwigPyPacked_str, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigpacked_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + 0, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpypacked_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpypacked_type) < 0) + return NULL; + } + return &swigpypacked_type; +} + +SWIGRUNTIME PyObject * +SwigPyPacked_New(void *ptr, size_t size, swig_type_info *ty) +{ + SwigPyPacked *sobj = PyObject_NEW(SwigPyPacked, SwigPyPacked_type()); + if (sobj) { + void *pack = malloc(size); + if (pack) { + memcpy(pack, ptr, size); + sobj->pack = pack; + sobj->ty = ty; + sobj->size = size; + } else { + PyObject_DEL((PyObject *) sobj); + sobj = 0; + } + } + return (PyObject *) sobj; +} + +SWIGRUNTIME swig_type_info * +SwigPyPacked_UnpackData(PyObject *obj, void *ptr, size_t size) +{ + if (SwigPyPacked_Check(obj)) { + SwigPyPacked *sobj = (SwigPyPacked *)obj; + if (sobj->size != size) return 0; + memcpy(ptr, sobj->pack, size); + return sobj->ty; + } else { + return 0; + } +} + +/* ----------------------------------------------------------------------------- + * pointers/data manipulation + * ----------------------------------------------------------------------------- */ + +static PyObject *Swig_This_global = NULL; + +SWIGRUNTIME PyObject * +SWIG_This(void) +{ + if (Swig_This_global == NULL) + Swig_This_global = SWIG_Python_str_FromChar("this"); + return Swig_This_global; +} + +/* #define SWIG_PYTHON_SLOW_GETSET_THIS */ + +/* TODO: I don't know how to implement the fast getset in Python 3 right now */ +#if PY_VERSION_HEX>=0x03000000 +#define SWIG_PYTHON_SLOW_GETSET_THIS +#endif + +SWIGRUNTIME SwigPyObject * +SWIG_Python_GetSwigThis(PyObject *pyobj) +{ + PyObject *obj; + + if (SwigPyObject_Check(pyobj)) + return (SwigPyObject *) pyobj; + +#ifdef SWIGPYTHON_BUILTIN + (void)obj; +# ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + pyobj = PyWeakref_GET_OBJECT(pyobj); + if (pyobj && SwigPyObject_Check(pyobj)) + return (SwigPyObject*) pyobj; + } +# endif + return NULL; +#else + + obj = 0; + +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + if (PyInstance_Check(pyobj)) { + obj = _PyInstance_Lookup(pyobj, SWIG_This()); + } else { + PyObject **dictptr = _PyObject_GetDictPtr(pyobj); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + obj = dict ? PyDict_GetItem(dict, SWIG_This()) : 0; + } else { +#ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + PyObject *wobj = PyWeakref_GET_OBJECT(pyobj); + return wobj ? SWIG_Python_GetSwigThis(wobj) : 0; + } +#endif + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } + } + } +#else + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } +#endif + if (obj && !SwigPyObject_Check(obj)) { + /* a PyObject is called 'this', try to get the 'real this' + SwigPyObject from it */ + return SWIG_Python_GetSwigThis(obj); + } + return (SwigPyObject *)obj; +#endif +} + +/* Acquire a pointer value */ + +SWIGRUNTIME int +SWIG_Python_AcquirePtr(PyObject *obj, int own) { + if (own == SWIG_POINTER_OWN) { + SwigPyObject *sobj = SWIG_Python_GetSwigThis(obj); + if (sobj) { + int oldown = sobj->own; + sobj->own = own; + return oldown; + } + } + return 0; +} + +/* Convert a pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPtrAndOwn(PyObject *obj, void **ptr, swig_type_info *ty, int flags, int *own) { + int res; + SwigPyObject *sobj; + int implicit_conv = (flags & SWIG_POINTER_IMPLICIT_CONV) != 0; + + if (!obj) + return SWIG_ERROR; + if (obj == Py_None && !implicit_conv) { + if (ptr) + *ptr = 0; + return (flags & SWIG_POINTER_NO_NULL) ? SWIG_NullReferenceError : SWIG_OK; + } + + res = SWIG_ERROR; + + sobj = SWIG_Python_GetSwigThis(obj); + if (own) + *own = 0; + while (sobj) { + void *vptr = sobj->ptr; + if (ty) { + swig_type_info *to = sobj->ty; + if (to == ty) { + /* no type cast needed */ + if (ptr) *ptr = vptr; + break; + } else { + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) { + sobj = (SwigPyObject *)sobj->next; + } else { + if (ptr) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + if (newmemory == SWIG_CAST_NEW_MEMORY) { + assert(own); /* badly formed typemap which will lead to a memory leak - it must set and use own to delete *ptr */ + if (own) + *own = *own | SWIG_CAST_NEW_MEMORY; + } + } + break; + } + } + } else { + if (ptr) *ptr = vptr; + break; + } + } + if (sobj) { + if (own) + *own = *own | sobj->own; + if (flags & SWIG_POINTER_DISOWN) { + sobj->own = 0; + } + res = SWIG_OK; + } else { + if (implicit_conv) { + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + if (data && !data->implicitconv) { + PyObject *klass = data->klass; + if (klass) { + PyObject *impconv; + data->implicitconv = 1; /* avoid recursion and call 'explicit' constructors*/ + impconv = SWIG_Python_CallFunctor(klass, obj); + data->implicitconv = 0; + if (PyErr_Occurred()) { + PyErr_Clear(); + impconv = 0; + } + if (impconv) { + SwigPyObject *iobj = SWIG_Python_GetSwigThis(impconv); + if (iobj) { + void *vptr; + res = SWIG_Python_ConvertPtrAndOwn((PyObject*)iobj, &vptr, ty, 0, 0); + if (SWIG_IsOK(res)) { + if (ptr) { + *ptr = vptr; + /* transfer the ownership to 'ptr' */ + iobj->own = 0; + res = SWIG_AddCast(res); + res = SWIG_AddNewMask(res); + } else { + res = SWIG_AddCast(res); + } + } + } + Py_DECREF(impconv); + } + } + } + if (!SWIG_IsOK(res) && obj == Py_None) { + if (ptr) + *ptr = 0; + if (PyErr_Occurred()) + PyErr_Clear(); + res = SWIG_OK; + } + } + } + return res; +} + +/* Convert a function ptr value */ + +SWIGRUNTIME int +SWIG_Python_ConvertFunctionPtr(PyObject *obj, void **ptr, swig_type_info *ty) { + if (!PyCFunction_Check(obj)) { + return SWIG_ConvertPtr(obj, ptr, ty, 0); + } else { + void *vptr = 0; + swig_cast_info *tc; + + /* here we get the method pointer for callbacks */ + const char *doc = (((PyCFunctionObject *)obj) -> m_ml -> ml_doc); + const char *desc = doc ? strstr(doc, "swig_ptr: ") : 0; + if (desc) + desc = ty ? SWIG_UnpackVoidPtr(desc + 10, &vptr, ty->name) : 0; + if (!desc) + return SWIG_ERROR; + tc = SWIG_TypeCheck(desc,ty); + if (tc) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + assert(!newmemory); /* newmemory handling not yet implemented */ + } else { + return SWIG_ERROR; + } + return SWIG_OK; + } +} + +/* Convert a packed pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPacked(PyObject *obj, void *ptr, size_t sz, swig_type_info *ty) { + swig_type_info *to = SwigPyPacked_UnpackData(obj, ptr, sz); + if (!to) return SWIG_ERROR; + if (ty) { + if (to != ty) { + /* check type cast? */ + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) return SWIG_ERROR; + } + } + return SWIG_OK; +} + +/* ----------------------------------------------------------------------------- + * Create a new pointer object + * ----------------------------------------------------------------------------- */ + +/* + Create a new instance object, without calling __init__, and set the + 'this' attribute. +*/ + +SWIGRUNTIME PyObject* +SWIG_Python_NewShadowInstance(SwigPyClientData *data, PyObject *swig_this) +{ + PyObject *inst = 0; + PyObject *newraw = data->newraw; + if (newraw) { + inst = PyObject_Call(newraw, data->newargs, NULL); + if (inst) { +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + PyDict_SetItem(dict, SWIG_This(), swig_this); + } + } +#else + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } +#endif + } + } else { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *empty_args = PyTuple_New(0); + if (empty_args) { + PyObject *empty_kwargs = PyDict_New(); + if (empty_kwargs) { + inst = ((PyTypeObject *)data->newargs)->tp_new((PyTypeObject *)data->newargs, empty_args, empty_kwargs); + Py_DECREF(empty_kwargs); + if (inst) { + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } else { + Py_TYPE(inst)->tp_flags &= ~Py_TPFLAGS_VALID_VERSION_TAG; + } + } + } + Py_DECREF(empty_args); + } +#else + PyObject *dict = PyDict_New(); + if (dict) { + PyDict_SetItem(dict, SWIG_This(), swig_this); + inst = PyInstance_NewRaw(data->newargs, dict); + Py_DECREF(dict); + } +#endif + } + return inst; +} + +SWIGRUNTIME int +SWIG_Python_SetSwigThis(PyObject *inst, PyObject *swig_this) +{ +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + } + return PyDict_SetItem(dict, SWIG_This(), swig_this); + } +#endif + return PyObject_SetAttr(inst, SWIG_This(), swig_this); +} + + +SWIGINTERN PyObject * +SWIG_Python_InitShadowInstance(PyObject *args) { + PyObject *obj[2]; + if (!SWIG_Python_UnpackTuple(args, "swiginit", 2, 2, obj)) { + return NULL; + } else { + SwigPyObject *sthis = SWIG_Python_GetSwigThis(obj[0]); + if (sthis) { + SwigPyObject_append((PyObject*) sthis, obj[1]); + } else { + if (SWIG_Python_SetSwigThis(obj[0], obj[1]) != 0) + return NULL; + } + return SWIG_Py_Void(); + } +} + +/* Create a new pointer object */ + +SWIGRUNTIME PyObject * +SWIG_Python_NewPointerObj(PyObject *self, void *ptr, swig_type_info *type, int flags) { + SwigPyClientData *clientdata; + PyObject * robj; + int own; + + if (!ptr) + return SWIG_Py_Void(); + + clientdata = type ? (SwigPyClientData *)(type->clientdata) : 0; + own = (flags & SWIG_POINTER_OWN) ? SWIG_POINTER_OWN : 0; + if (clientdata && clientdata->pytype) { + SwigPyObject *newobj; + if (flags & SWIG_BUILTIN_TP_INIT) { + newobj = (SwigPyObject*) self; + if (newobj->ptr) { + PyObject *next_self = clientdata->pytype->tp_alloc(clientdata->pytype, 0); + while (newobj->next) + newobj = (SwigPyObject *) newobj->next; + newobj->next = next_self; + newobj = (SwigPyObject *)next_self; +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + } else { + newobj = PyObject_New(SwigPyObject, clientdata->pytype); +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + if (newobj) { + newobj->ptr = ptr; + newobj->ty = type; + newobj->own = own; + newobj->next = 0; + return (PyObject*) newobj; + } + return SWIG_Py_Void(); + } + + assert(!(flags & SWIG_BUILTIN_TP_INIT)); + + robj = SwigPyObject_New(ptr, type, own); + if (robj && clientdata && !(flags & SWIG_POINTER_NOSHADOW)) { + PyObject *inst = SWIG_Python_NewShadowInstance(clientdata, robj); + Py_DECREF(robj); + robj = inst; + } + return robj; +} + +/* Create a new packed object */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_NewPackedObj(void *ptr, size_t sz, swig_type_info *type) { + return ptr ? SwigPyPacked_New((void *) ptr, sz, type) : SWIG_Py_Void(); +} + +/* -----------------------------------------------------------------------------* + * Get type list + * -----------------------------------------------------------------------------*/ + +#ifdef SWIG_LINK_RUNTIME +void *SWIG_ReturnGlobalTypeList(void *); +#endif + +SWIGRUNTIME swig_module_info * +SWIG_Python_GetModule(void *SWIGUNUSEDPARM(clientdata)) { + static void *type_pointer = (void *)0; + /* first check if module already created */ + if (!type_pointer) { +#ifdef SWIG_LINK_RUNTIME + type_pointer = SWIG_ReturnGlobalTypeList((void *)0); +#else + type_pointer = PyCapsule_Import(SWIGPY_CAPSULE_NAME, 0); + if (PyErr_Occurred()) { + PyErr_Clear(); + type_pointer = (void *)0; + } +#endif + } + return (swig_module_info *) type_pointer; +} + +SWIGRUNTIME void +SWIG_Python_DestroyModule(PyObject *obj) +{ + swig_module_info *swig_module = (swig_module_info *) PyCapsule_GetPointer(obj, SWIGPY_CAPSULE_NAME); + swig_type_info **types = swig_module->types; + size_t i; + for (i =0; i < swig_module->size; ++i) { + swig_type_info *ty = types[i]; + if (ty->owndata) { + SwigPyClientData *data = (SwigPyClientData *) ty->clientdata; + if (data) SwigPyClientData_Del(data); + } + } + Py_DECREF(SWIG_This()); + Swig_This_global = NULL; +} + +SWIGRUNTIME void +SWIG_Python_SetModule(swig_module_info *swig_module) { +#if PY_VERSION_HEX >= 0x03000000 + /* Add a dummy module object into sys.modules */ + PyObject *module = PyImport_AddModule("swig_runtime_data" SWIG_RUNTIME_VERSION); +#else + static PyMethodDef swig_empty_runtime_method_table[] = { {NULL, NULL, 0, NULL} }; /* Sentinel */ + PyObject *module = Py_InitModule("swig_runtime_data" SWIG_RUNTIME_VERSION, swig_empty_runtime_method_table); +#endif + PyObject *pointer = PyCapsule_New((void *) swig_module, SWIGPY_CAPSULE_NAME, SWIG_Python_DestroyModule); + if (pointer && module) { + PyModule_AddObject(module, "type_pointer_capsule" SWIG_TYPE_TABLE_NAME, pointer); + } else { + Py_XDECREF(pointer); + } +} + +/* The python cached type query */ +SWIGRUNTIME PyObject * +SWIG_Python_TypeCache(void) { + static PyObject *SWIG_STATIC_POINTER(cache) = PyDict_New(); + return cache; +} + +SWIGRUNTIME swig_type_info * +SWIG_Python_TypeQuery(const char *type) +{ + PyObject *cache = SWIG_Python_TypeCache(); + PyObject *key = SWIG_Python_str_FromChar(type); + PyObject *obj = PyDict_GetItem(cache, key); + swig_type_info *descriptor; + if (obj) { + descriptor = (swig_type_info *) PyCapsule_GetPointer(obj, NULL); + } else { + swig_module_info *swig_module = SWIG_GetModule(0); + descriptor = SWIG_TypeQueryModule(swig_module, swig_module, type); + if (descriptor) { + obj = PyCapsule_New((void*) descriptor, NULL, NULL); + PyDict_SetItem(cache, key, obj); + Py_DECREF(obj); + } + } + Py_DECREF(key); + return descriptor; +} + +/* + For backward compatibility only +*/ +#define SWIG_POINTER_EXCEPTION 0 +#define SWIG_arg_fail(arg) SWIG_Python_ArgFail(arg) +#define SWIG_MustGetPtr(p, type, argnum, flags) SWIG_Python_MustGetPtr(p, type, argnum, flags) + +SWIGRUNTIME int +SWIG_Python_AddErrMesg(const char* mesg, int infront) +{ + if (PyErr_Occurred()) { + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + const char *errmesg = tmp ? tmp : "Invalid error message"; + Py_XINCREF(type); + PyErr_Clear(); + if (infront) { + PyErr_Format(type, "%s %s", mesg, errmesg); + } else { + PyErr_Format(type, "%s %s", errmesg, mesg); + } + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + } + return 1; + } else { + return 0; + } +} + +SWIGRUNTIME int +SWIG_Python_ArgFail(int argnum) +{ + if (PyErr_Occurred()) { + /* add information about failing argument */ + char mesg[256]; + PyOS_snprintf(mesg, sizeof(mesg), "argument number %d:", argnum); + return SWIG_Python_AddErrMesg(mesg, 1); + } else { + return 0; + } +} + +SWIGRUNTIMEINLINE const char * +SwigPyObject_GetDesc(PyObject *self) +{ + SwigPyObject *v = (SwigPyObject *)self; + swig_type_info *ty = v ? v->ty : 0; + return ty ? ty->str : ""; +} + +SWIGRUNTIME void +SWIG_Python_TypeError(const char *type, PyObject *obj) +{ + if (type) { +#if defined(SWIG_COBJECT_TYPES) + if (obj && SwigPyObject_Check(obj)) { + const char *otype = (const char *) SwigPyObject_GetDesc(obj); + if (otype) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, 'SwigPyObject(%s)' is received", + type, otype); + return; + } + } else +#endif + { + const char *otype = (obj ? obj->ob_type->tp_name : 0); + if (otype) { + PyObject *str = PyObject_Str(obj); + const char *cstr = str ? SWIG_Python_str_AsChar(str) : 0; + if (cstr) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s(%s)' is received", + type, otype, cstr); + SWIG_Python_str_DelForPy3(cstr); + } else { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s' is received", + type, otype); + } + Py_XDECREF(str); + return; + } + } + PyErr_Format(PyExc_TypeError, "a '%s' is expected", type); + } else { + PyErr_Format(PyExc_TypeError, "unexpected type is received"); + } +} + + +/* Convert a pointer value, signal an exception on a type mismatch */ +SWIGRUNTIME void * +SWIG_Python_MustGetPtr(PyObject *obj, swig_type_info *ty, int SWIGUNUSEDPARM(argnum), int flags) { + void *result; + if (SWIG_Python_ConvertPtr(obj, &result, ty, flags) == -1) { + PyErr_Clear(); +#if SWIG_POINTER_EXCEPTION + if (flags) { + SWIG_Python_TypeError(SWIG_TypePrettyName(ty), obj); + SWIG_Python_ArgFail(argnum); + } +#endif + } + return result; +} + +#ifdef SWIGPYTHON_BUILTIN +SWIGRUNTIME int +SWIG_Python_NonDynamicSetAttr(PyObject *obj, PyObject *name, PyObject *value) { + PyTypeObject *tp = obj->ob_type; + PyObject *descr; + PyObject *encoded_name; + descrsetfunc f; + int res = -1; + +# ifdef Py_USING_UNICODE + if (PyString_Check(name)) { + name = PyUnicode_Decode(PyString_AsString(name), PyString_Size(name), NULL, NULL); + if (!name) + return -1; + } else if (!PyUnicode_Check(name)) +# else + if (!PyString_Check(name)) +# endif + { + PyErr_Format(PyExc_TypeError, "attribute name must be string, not '%.200s'", name->ob_type->tp_name); + return -1; + } else { + Py_INCREF(name); + } + + if (!tp->tp_dict) { + if (PyType_Ready(tp) < 0) + goto done; + } + + descr = _PyType_Lookup(tp, name); + f = NULL; + if (descr != NULL) + f = descr->ob_type->tp_descr_set; + if (!f) { + if (PyString_Check(name)) { + encoded_name = name; + Py_INCREF(name); + } else { + encoded_name = PyUnicode_AsUTF8String(name); + if (!encoded_name) + return -1; + } + PyErr_Format(PyExc_AttributeError, "'%.100s' object has no attribute '%.200s'", tp->tp_name, PyString_AsString(encoded_name)); + Py_DECREF(encoded_name); + } else { + res = f(descr, obj, value); + } + + done: + Py_DECREF(name); + return res; +} +#endif + + +#ifdef __cplusplus +} +#endif + + + +#define SWIG_exception_fail(code, msg) do { SWIG_Error(code, msg); SWIG_fail; } while(0) + +#define SWIG_contract_assert(expr, msg) if (!(expr)) { SWIG_Error(SWIG_RuntimeError, msg); SWIG_fail; } else + + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Method creation and docstring support functions */ + +SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name); +SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); +SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); + +#ifdef __cplusplus +} +#endif + + +/* -------- TYPES TABLE (BEGIN) -------- */ + +#define SWIGTYPE_p_ControlMode swig_types[0] +#define SWIGTYPE_p_CoordinateSystem swig_types[1] +#define SWIGTYPE_p_EngineType swig_types[2] +#define SWIGTYPE_p_IndicatorState swig_types[3] +#define SWIGTYPE_p_Mode swig_types[4] +#define SWIGTYPE_p_SimulationMode swig_types[5] +#define SWIGTYPE_p_Type swig_types[6] +#define SWIGTYPE_p_UserInputEvent swig_types[7] +#define SWIGTYPE_p_WbCameraRecognitionObject swig_types[8] +#define SWIGTYPE_p_WbContactPoint swig_types[9] +#define SWIGTYPE_p_WbLidarPoint swig_types[10] +#define SWIGTYPE_p_WbMouseState swig_types[11] +#define SWIGTYPE_p_WbRadarTarget swig_types[12] +#define SWIGTYPE_p_WheelIndex swig_types[13] +#define SWIGTYPE_p_WiperMode swig_types[14] +#define SWIGTYPE_p_char swig_types[15] +#define SWIGTYPE_p_webots__Car swig_types[16] +#define SWIGTYPE_p_webots__Driver swig_types[17] +#define SWIGTYPE_p_webots__Robot swig_types[18] +#define SWIGTYPE_p_webots__Supervisor swig_types[19] +static swig_type_info *swig_types[21]; +static swig_module_info swig_module = {swig_types, 20, 0, 0, 0, 0}; +#define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name) +#define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name) + +/* -------- TYPES TABLE (END) -------- */ + +#ifdef SWIG_TypeQuery +# undef SWIG_TypeQuery +#endif +#define SWIG_TypeQuery SWIG_Python_TypeQuery + +/*----------------------------------------------- + @(target):= _vehicle.so + ------------------------------------------------*/ +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_init PyInit__vehicle + +#else +# define SWIG_init init_vehicle + +#endif +#define SWIG_name "_vehicle" + +#define SWIGVERSION 0x040002 +#define SWIG_VERSION SWIGVERSION + + +#define SWIG_as_voidptr(a) const_cast< void * >(static_cast< const void * >(a)) +#define SWIG_as_voidptrptr(a) ((void)SWIG_as_voidptr(*a),reinterpret_cast< void** >(a)) + + +#include + + +namespace swig { + class SwigPtr_PyObject { + protected: + PyObject *_obj; + + public: + SwigPtr_PyObject() :_obj(0) + { + } + + SwigPtr_PyObject(const SwigPtr_PyObject& item) : _obj(item._obj) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + SwigPtr_PyObject(PyObject *obj, bool initial_ref = true) :_obj(obj) + { + if (initial_ref) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + } + + SwigPtr_PyObject & operator=(const SwigPtr_PyObject& item) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(item._obj); + Py_XDECREF(_obj); + _obj = item._obj; + SWIG_PYTHON_THREAD_END_BLOCK; + return *this; + } + + ~SwigPtr_PyObject() + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XDECREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + operator PyObject *() const + { + return _obj; + } + + PyObject *operator->() const + { + return _obj; + } + }; +} + + +namespace swig { + struct SwigVar_PyObject : SwigPtr_PyObject { + SwigVar_PyObject(PyObject* obj = 0) : SwigPtr_PyObject(obj, false) { } + + SwigVar_PyObject & operator = (PyObject* obj) + { + Py_XDECREF(_obj); + _obj = obj; + return *this; + } + }; +} + + +#include +#include + + +SWIGINTERNINLINE PyObject* + SWIG_From_int (int value) +{ + return PyInt_FromLong((long) value); +} + + +SWIGINTERNINLINE PyObject* + SWIG_From_bool (bool value) +{ + return PyBool_FromLong(value ? 1 : 0); +} + + +SWIGINTERN int +SWIG_AsVal_double (PyObject *obj, double *val) +{ + int res = SWIG_TypeError; + if (PyFloat_Check(obj)) { + if (val) *val = PyFloat_AsDouble(obj); + return SWIG_OK; +#if PY_VERSION_HEX < 0x03000000 + } else if (PyInt_Check(obj)) { + if (val) *val = (double) PyInt_AsLong(obj); + return SWIG_OK; +#endif + } else if (PyLong_Check(obj)) { + double v = PyLong_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + double d = PyFloat_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = d; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_AddCast(SWIG_OK)); + } else { + PyErr_Clear(); + } + } + } +#endif + return res; +} + + + #define SWIG_From_double PyFloat_FromDouble + + +#include +#if !defined(SWIG_NO_LLONG_MAX) +# if !defined(LLONG_MAX) && defined(__GNUC__) && defined (__LONG_LONG_MAX__) +# define LLONG_MAX __LONG_LONG_MAX__ +# define LLONG_MIN (-LLONG_MAX - 1LL) +# define ULLONG_MAX (LLONG_MAX * 2ULL + 1ULL) +# endif +#endif + + +#include + + +#include + + +SWIGINTERNINLINE int +SWIG_CanCastAsInteger(double *d, double min, double max) { + double x = *d; + if ((min <= x && x <= max)) { + double fx = floor(x); + double cx = ceil(x); + double rd = ((x - fx) < 0.5) ? fx : cx; /* simple rint */ + if ((errno == EDOM) || (errno == ERANGE)) { + errno = 0; + } else { + double summ, reps, diff; + if (rd < x) { + diff = x - rd; + } else if (rd > x) { + diff = rd - x; + } else { + return 1; + } + summ = rd + x; + reps = diff/summ; + if (reps < 8*DBL_EPSILON) { + *d = rd; + return 1; + } + } + } + return 0; +} + + +SWIGINTERN int +SWIG_AsVal_long (PyObject *obj, long* val) +{ +#if PY_VERSION_HEX < 0x03000000 + if (PyInt_Check(obj)) { + if (val) *val = PyInt_AsLong(obj); + return SWIG_OK; + } else +#endif + if (PyLong_Check(obj)) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + return SWIG_OverflowError; + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + long v = PyInt_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + double d; + int res = SWIG_AddCast(SWIG_AsVal_double (obj,&d)); + if (SWIG_IsOK(res) && SWIG_CanCastAsInteger(&d, LONG_MIN, LONG_MAX)) { + if (val) *val = (long)(d); + return res; + } + } + } +#endif + return SWIG_TypeError; +} + + +SWIGINTERN int +SWIG_AsVal_int (PyObject * obj, int *val) +{ + long v; + int res = SWIG_AsVal_long (obj, &v); + if (SWIG_IsOK(res)) { + if ((v < INT_MIN || v > INT_MAX)) { + return SWIG_OverflowError; + } else { + if (val) *val = static_cast< int >(v); + } + } + return res; +} + + +SWIGINTERN int +SWIG_AsVal_bool (PyObject *obj, bool *val) +{ + int r; + if (!PyBool_Check(obj)) + return SWIG_ERROR; + r = PyObject_IsTrue(obj); + if (r == -1) + return SWIG_ERROR; + if (val) *val = r ? true : false; + return SWIG_OK; +} + +#ifdef __cplusplus +extern "C" { +#endif +SWIGINTERN PyObject *_wrap_new_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Driver", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)new webots::Driver(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDriverInstance(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_getDriverInstance", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)webots::Driver::getDriverInstance(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, 0 | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Driver" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_isInitialisationPossible(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + bool result; + + if (!SWIG_Python_UnpackTuple(args, "Driver_isInitialisationPossible", 0, 0, 0)) SWIG_fail; + result = (bool)webots::Driver::isInitialisationPossible(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_step(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_step" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->step(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setCruisingSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setCruisingSpeed" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setCruisingSpeed(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getTargetCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getTargetCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getTargetCruisingSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getCurrentSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getCurrentSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getCurrentSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setThrottle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setThrottle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setThrottle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getThrottle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrakeIntensity", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrakeIntensity" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrakeIntensity(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getBrakeIntensity(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::IndicatorState arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setIndicator", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setIndicator" "', argument " "2"" of type '" "webots::Driver::IndicatorState""'"); + } + arg2 = static_cast< webots::Driver::IndicatorState >(val2); + (arg1)->setIndicator(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setHazardFlashers", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setHazardFlashers" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setHazardFlashers(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::IndicatorState result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::IndicatorState)(arg1)->getIndicator(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getHazardFlashers(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setDippedBeams", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setDippedBeams" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setDippedBeams(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setAntifogLights", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setAntifogLights" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setAntifogLights(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getDippedBeams(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getAntifogLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getRpm(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getRpm" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getRpm(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGear(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + int arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setGear", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setGear" "', argument " "2"" of type '" "int""'"); + } + arg2 = static_cast< int >(val2); + (arg1)->setGear(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGearNumber(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGearNumber" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGearNumber(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getControlMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::ControlMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getControlMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::ControlMode)(arg1)->getControlMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWiperMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWiperMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWiperMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWiperMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrake(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrake", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrake" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrake" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrake(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWipersMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWipersMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWipersMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWipersMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Driver_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Driver, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Driver_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +SWIGINTERN PyObject *_wrap_new_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Car", 0, 0, 0)) SWIG_fail; + result = (webots::Car *)new webots::Car(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Car, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Car" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::Type result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::Type)(arg1)->getType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getEngineType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::EngineType result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getEngineType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::EngineType)(arg1)->getEngineType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setIndicatorPeriod", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setIndicatorPeriod" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setIndicatorPeriod(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getIndicatorPeriod(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBackwardsLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBackwardsLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBackwardsLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBrakeLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBrakeLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBrakeLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackFront(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackFront" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackFront(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackRear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackRear" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackRear(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelbase(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelbase" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getWheelbase(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getFrontWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getFrontWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getFrontWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRearWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRearWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRearWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelEncoder(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelEncoder", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelEncoder" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelEncoder" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelEncoder(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelSpeed" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelSpeed" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelSpeed(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setLeftSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setLeftSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setLeftSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setRightSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setRightSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setRightSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRightSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getLeftSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableLimitedSlipDifferential(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableLimitedSlipDifferential", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableLimitedSlipDifferential(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableIndicatorAutoDisabling(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableIndicatorAutoDisabling", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableIndicatorAutoDisabling(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Car_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Car, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Car_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +static PyMethodDef SwigMethods[] = { + { "SWIG_PyInstanceMethod_New", SWIG_PyInstanceMethod_New, METH_O, NULL}, + { "new_Driver", _wrap_new_Driver, METH_NOARGS, NULL}, + { "Driver_getDriverInstance", _wrap_Driver_getDriverInstance, METH_NOARGS, NULL}, + { "delete_Driver", _wrap_delete_Driver, METH_O, NULL}, + { "Driver_isInitialisationPossible", _wrap_Driver_isInitialisationPossible, METH_NOARGS, NULL}, + { "Driver_step", _wrap_Driver_step, METH_O, NULL}, + { "Driver_setSteeringAngle", _wrap_Driver_setSteeringAngle, METH_VARARGS, NULL}, + { "Driver_getSteeringAngle", _wrap_Driver_getSteeringAngle, METH_O, NULL}, + { "Driver_setCruisingSpeed", _wrap_Driver_setCruisingSpeed, METH_VARARGS, NULL}, + { "Driver_getTargetCruisingSpeed", _wrap_Driver_getTargetCruisingSpeed, METH_O, NULL}, + { "Driver_getCurrentSpeed", _wrap_Driver_getCurrentSpeed, METH_O, NULL}, + { "Driver_setThrottle", _wrap_Driver_setThrottle, METH_VARARGS, NULL}, + { "Driver_getThrottle", _wrap_Driver_getThrottle, METH_O, NULL}, + { "Driver_setBrakeIntensity", _wrap_Driver_setBrakeIntensity, METH_VARARGS, NULL}, + { "Driver_getBrakeIntensity", _wrap_Driver_getBrakeIntensity, METH_O, NULL}, + { "Driver_setIndicator", _wrap_Driver_setIndicator, METH_VARARGS, NULL}, + { "Driver_setHazardFlashers", _wrap_Driver_setHazardFlashers, METH_VARARGS, NULL}, + { "Driver_getIndicator", _wrap_Driver_getIndicator, METH_O, NULL}, + { "Driver_getHazardFlashers", _wrap_Driver_getHazardFlashers, METH_O, NULL}, + { "Driver_setDippedBeams", _wrap_Driver_setDippedBeams, METH_VARARGS, NULL}, + { "Driver_setAntifogLights", _wrap_Driver_setAntifogLights, METH_VARARGS, NULL}, + { "Driver_getDippedBeams", _wrap_Driver_getDippedBeams, METH_O, NULL}, + { "Driver_getAntifogLights", _wrap_Driver_getAntifogLights, METH_O, NULL}, + { "Driver_getRpm", _wrap_Driver_getRpm, METH_O, NULL}, + { "Driver_getGear", _wrap_Driver_getGear, METH_O, NULL}, + { "Driver_setGear", _wrap_Driver_setGear, METH_VARARGS, NULL}, + { "Driver_getGearNumber", _wrap_Driver_getGearNumber, METH_O, NULL}, + { "Driver_getControlMode", _wrap_Driver_getControlMode, METH_O, NULL}, + { "Driver_setWiperMode", _wrap_Driver_setWiperMode, METH_VARARGS, NULL}, + { "Driver_getWiperMode", _wrap_Driver_getWiperMode, METH_O, NULL}, + { "Driver_setBrake", _wrap_Driver_setBrake, METH_VARARGS, NULL}, + { "Driver_setWipersMode", _wrap_Driver_setWipersMode, METH_VARARGS, NULL}, + { "Driver_getWipersMode", _wrap_Driver_getWipersMode, METH_O, NULL}, + { "Driver_swigregister", Driver_swigregister, METH_O, NULL}, + { "Driver_swiginit", Driver_swiginit, METH_VARARGS, NULL}, + { "new_Car", _wrap_new_Car, METH_NOARGS, NULL}, + { "delete_Car", _wrap_delete_Car, METH_O, NULL}, + { "Car_getType", _wrap_Car_getType, METH_O, NULL}, + { "Car_getEngineType", _wrap_Car_getEngineType, METH_O, NULL}, + { "Car_setIndicatorPeriod", _wrap_Car_setIndicatorPeriod, METH_VARARGS, NULL}, + { "Car_getIndicatorPeriod", _wrap_Car_getIndicatorPeriod, METH_O, NULL}, + { "Car_getBackwardsLights", _wrap_Car_getBackwardsLights, METH_O, NULL}, + { "Car_getBrakeLights", _wrap_Car_getBrakeLights, METH_O, NULL}, + { "Car_getTrackFront", _wrap_Car_getTrackFront, METH_O, NULL}, + { "Car_getTrackRear", _wrap_Car_getTrackRear, METH_O, NULL}, + { "Car_getWheelbase", _wrap_Car_getWheelbase, METH_O, NULL}, + { "Car_getFrontWheelRadius", _wrap_Car_getFrontWheelRadius, METH_O, NULL}, + { "Car_getRearWheelRadius", _wrap_Car_getRearWheelRadius, METH_O, NULL}, + { "Car_getWheelEncoder", _wrap_Car_getWheelEncoder, METH_VARARGS, NULL}, + { "Car_getWheelSpeed", _wrap_Car_getWheelSpeed, METH_VARARGS, NULL}, + { "Car_setLeftSteeringAngle", _wrap_Car_setLeftSteeringAngle, METH_VARARGS, NULL}, + { "Car_setRightSteeringAngle", _wrap_Car_setRightSteeringAngle, METH_VARARGS, NULL}, + { "Car_getRightSteeringAngle", _wrap_Car_getRightSteeringAngle, METH_O, NULL}, + { "Car_getLeftSteeringAngle", _wrap_Car_getLeftSteeringAngle, METH_O, NULL}, + { "Car_enableLimitedSlipDifferential", _wrap_Car_enableLimitedSlipDifferential, METH_VARARGS, NULL}, + { "Car_enableIndicatorAutoDisabling", _wrap_Car_enableIndicatorAutoDisabling, METH_VARARGS, NULL}, + { "Car_swigregister", Car_swigregister, METH_O, NULL}, + { "Car_swiginit", Car_swiginit, METH_VARARGS, NULL}, + { NULL, NULL, 0, NULL } +}; + +static PyMethodDef SwigMethods_proxydocs[] = { + { NULL, NULL, 0, NULL } +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (BEGIN) -------- */ + +static void *_p_webots__DriverTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__CarTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) (webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__DriverTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__SupervisorTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) ((webots::Supervisor *) x)); +} +static void *_p_webots__CarTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *)(webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__CarTo_p_webots__Driver(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Driver *) ((webots::Car *) x)); +} +static swig_type_info _swigt__p_ControlMode = {"_p_ControlMode", "ControlMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_CoordinateSystem = {"_p_CoordinateSystem", "CoordinateSystem *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_EngineType = {"_p_EngineType", "EngineType *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_IndicatorState = {"_p_IndicatorState", "IndicatorState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Mode = {"_p_Mode", "Mode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_SimulationMode = {"_p_SimulationMode", "SimulationMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Type = {"_p_Type", "Type *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_UserInputEvent = {"_p_UserInputEvent", "UserInputEvent *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbCameraRecognitionObject = {"_p_WbCameraRecognitionObject", "WbCameraRecognitionObject *|webots::CameraRecognitionObject *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbContactPoint = {"_p_WbContactPoint", "WbContactPoint *|webots::ContactPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbLidarPoint = {"_p_WbLidarPoint", "WbLidarPoint *|webots::LidarPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbMouseState = {"_p_WbMouseState", "WbMouseState *|webots::MouseState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbRadarTarget = {"_p_WbRadarTarget", "WbRadarTarget *|webots::RadarTarget *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WheelIndex = {"_p_WheelIndex", "WheelIndex *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WiperMode = {"_p_WiperMode", "WiperMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_char = {"_p_char", "char *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Car = {"_p_webots__Car", "webots::Car *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Driver = {"_p_webots__Driver", "webots::Driver *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Robot = {"_p_webots__Robot", "webots::Robot *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Supervisor = {"_p_webots__Supervisor", "webots::Supervisor *", 0, 0, (void*)0, 0}; + +static swig_type_info *swig_type_initial[] = { + &_swigt__p_ControlMode, + &_swigt__p_CoordinateSystem, + &_swigt__p_EngineType, + &_swigt__p_IndicatorState, + &_swigt__p_Mode, + &_swigt__p_SimulationMode, + &_swigt__p_Type, + &_swigt__p_UserInputEvent, + &_swigt__p_WbCameraRecognitionObject, + &_swigt__p_WbContactPoint, + &_swigt__p_WbLidarPoint, + &_swigt__p_WbMouseState, + &_swigt__p_WbRadarTarget, + &_swigt__p_WheelIndex, + &_swigt__p_WiperMode, + &_swigt__p_char, + &_swigt__p_webots__Car, + &_swigt__p_webots__Driver, + &_swigt__p_webots__Robot, + &_swigt__p_webots__Supervisor, +}; + +static swig_cast_info _swigc__p_ControlMode[] = { {&_swigt__p_ControlMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_CoordinateSystem[] = { {&_swigt__p_CoordinateSystem, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_EngineType[] = { {&_swigt__p_EngineType, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_IndicatorState[] = { {&_swigt__p_IndicatorState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Mode[] = { {&_swigt__p_Mode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_SimulationMode[] = { {&_swigt__p_SimulationMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Type[] = { {&_swigt__p_Type, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_UserInputEvent[] = { {&_swigt__p_UserInputEvent, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbCameraRecognitionObject[] = { {&_swigt__p_WbCameraRecognitionObject, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbContactPoint[] = { {&_swigt__p_WbContactPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbLidarPoint[] = { {&_swigt__p_WbLidarPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbMouseState[] = { {&_swigt__p_WbMouseState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbRadarTarget[] = { {&_swigt__p_WbRadarTarget, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WheelIndex[] = { {&_swigt__p_WheelIndex, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WiperMode[] = { {&_swigt__p_WiperMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_char[] = { {&_swigt__p_char, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Car[] = { {&_swigt__p_webots__Car, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Driver[] = { {&_swigt__p_webots__Driver, 0, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Driver, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Robot[] = { {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Supervisor, _p_webots__SupervisorTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Robot, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Supervisor[] = { {&_swigt__p_webots__Supervisor, 0, 0, 0}, {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Supervisor, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Supervisor, 0, 0},{0, 0, 0, 0}}; + +static swig_cast_info *swig_cast_initial[] = { + _swigc__p_ControlMode, + _swigc__p_CoordinateSystem, + _swigc__p_EngineType, + _swigc__p_IndicatorState, + _swigc__p_Mode, + _swigc__p_SimulationMode, + _swigc__p_Type, + _swigc__p_UserInputEvent, + _swigc__p_WbCameraRecognitionObject, + _swigc__p_WbContactPoint, + _swigc__p_WbLidarPoint, + _swigc__p_WbMouseState, + _swigc__p_WbRadarTarget, + _swigc__p_WheelIndex, + _swigc__p_WiperMode, + _swigc__p_char, + _swigc__p_webots__Car, + _swigc__p_webots__Driver, + _swigc__p_webots__Robot, + _swigc__p_webots__Supervisor, +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (END) -------- */ + +static swig_const_info swig_const_table[] = { +{0, 0, 0, 0.0, 0, 0}}; + +#ifdef __cplusplus +} +#endif +/* ----------------------------------------------------------------------------- + * Type initialization: + * This problem is tough by the requirement that no dynamic + * memory is used. Also, since swig_type_info structures store pointers to + * swig_cast_info structures and swig_cast_info structures store pointers back + * to swig_type_info structures, we need some lookup code at initialization. + * The idea is that swig generates all the structures that are needed. + * The runtime then collects these partially filled structures. + * The SWIG_InitializeModule function takes these initial arrays out of + * swig_module, and does all the lookup, filling in the swig_module.types + * array with the correct data and linking the correct swig_cast_info + * structures together. + * + * The generated swig_type_info structures are assigned statically to an initial + * array. We just loop through that array, and handle each type individually. + * First we lookup if this type has been already loaded, and if so, use the + * loaded structure instead of the generated one. Then we have to fill in the + * cast linked list. The cast data is initially stored in something like a + * two-dimensional array. Each row corresponds to a type (there are the same + * number of rows as there are in the swig_type_initial array). Each entry in + * a column is one of the swig_cast_info structures for that type. + * The cast_initial array is actually an array of arrays, because each row has + * a variable number of columns. So to actually build the cast linked list, + * we find the array of casts associated with the type, and loop through it + * adding the casts to the list. The one last trick we need to do is making + * sure the type pointer in the swig_cast_info struct is correct. + * + * First off, we lookup the cast->type name to see if it is already loaded. + * There are three cases to handle: + * 1) If the cast->type has already been loaded AND the type we are adding + * casting info to has not been loaded (it is in this module), THEN we + * replace the cast->type pointer with the type pointer that has already + * been loaded. + * 2) If BOTH types (the one we are adding casting info to, and the + * cast->type) are loaded, THEN the cast info has already been loaded by + * the previous module so we just ignore it. + * 3) Finally, if cast->type has not already been loaded, then we add that + * swig_cast_info to the linked list (because the cast->type) pointer will + * be correct. + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#if 0 +} /* c-mode */ +#endif +#endif + +#if 0 +#define SWIGRUNTIME_DEBUG +#endif + + +SWIGRUNTIME void +SWIG_InitializeModule(void *clientdata) { + size_t i; + swig_module_info *module_head, *iter; + int init; + + /* check to see if the circular list has been setup, if not, set it up */ + if (swig_module.next==0) { + /* Initialize the swig_module */ + swig_module.type_initial = swig_type_initial; + swig_module.cast_initial = swig_cast_initial; + swig_module.next = &swig_module; + init = 1; + } else { + init = 0; + } + + /* Try and load any already created modules */ + module_head = SWIG_GetModule(clientdata); + if (!module_head) { + /* This is the first module loaded for this interpreter */ + /* so set the swig module into the interpreter */ + SWIG_SetModule(clientdata, &swig_module); + } else { + /* the interpreter has loaded a SWIG module, but has it loaded this one? */ + iter=module_head; + do { + if (iter==&swig_module) { + /* Our module is already in the list, so there's nothing more to do. */ + return; + } + iter=iter->next; + } while (iter!= module_head); + + /* otherwise we must add our module into the list */ + swig_module.next = module_head->next; + module_head->next = &swig_module; + } + + /* When multiple interpreters are used, a module could have already been initialized in + a different interpreter, but not yet have a pointer in this interpreter. + In this case, we do not want to continue adding types... everything should be + set up already */ + if (init == 0) return; + + /* Now work on filling in swig_module.types */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: size %lu\n", (unsigned long)swig_module.size); +#endif + for (i = 0; i < swig_module.size; ++i) { + swig_type_info *type = 0; + swig_type_info *ret; + swig_cast_info *cast; + +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); +#endif + + /* if there is another module already loaded */ + if (swig_module.next != &swig_module) { + type = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, swig_module.type_initial[i]->name); + } + if (type) { + /* Overwrite clientdata field */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found type %s\n", type->name); +#endif + if (swig_module.type_initial[i]->clientdata) { + type->clientdata = swig_module.type_initial[i]->clientdata; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found and overwrite type %s \n", type->name); +#endif + } + } else { + type = swig_module.type_initial[i]; + } + + /* Insert casting types */ + cast = swig_module.cast_initial[i]; + while (cast->type) { + /* Don't need to add information already in the list */ + ret = 0; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: look cast %s\n", cast->type->name); +#endif + if (swig_module.next != &swig_module) { + ret = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, cast->type->name); +#ifdef SWIGRUNTIME_DEBUG + if (ret) printf("SWIG_InitializeModule: found cast %s\n", ret->name); +#endif + } + if (ret) { + if (type == swig_module.type_initial[i]) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: skip old type %s\n", ret->name); +#endif + cast->type = ret; + ret = 0; + } else { + /* Check for casting already in the list */ + swig_cast_info *ocast = SWIG_TypeCheck(ret->name, type); +#ifdef SWIGRUNTIME_DEBUG + if (ocast) printf("SWIG_InitializeModule: skip old cast %s\n", ret->name); +#endif + if (!ocast) ret = 0; + } + } + + if (!ret) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: adding cast %s\n", cast->type->name); +#endif + if (type->cast) { + type->cast->prev = cast; + cast->next = type->cast; + } + type->cast = cast; + } + cast++; + } + /* Set entry in modules->types array equal to the type */ + swig_module.types[i] = type; + } + swig_module.types[i] = 0; + +#ifdef SWIGRUNTIME_DEBUG + printf("**** SWIG_InitializeModule: Cast List ******\n"); + for (i = 0; i < swig_module.size; ++i) { + int j = 0; + swig_cast_info *cast = swig_module.cast_initial[i]; + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); + while (cast->type) { + printf("SWIG_InitializeModule: cast type %s\n", cast->type->name); + cast++; + ++j; + } + printf("---- Total casts: %d\n",j); + } + printf("**** SWIG_InitializeModule: Cast List ******\n"); +#endif +} + +/* This function will propagate the clientdata field of type to +* any new swig_type_info structures that have been added into the list +* of equivalent types. It is like calling +* SWIG_TypeClientData(type, clientdata) a second time. +*/ +SWIGRUNTIME void +SWIG_PropagateClientData(void) { + size_t i; + swig_cast_info *equiv; + static int init_run = 0; + + if (init_run) return; + init_run = 1; + + for (i = 0; i < swig_module.size; i++) { + if (swig_module.types[i]->clientdata) { + equiv = swig_module.types[i]->cast; + while (equiv) { + if (!equiv->converter) { + if (equiv->type && !equiv->type->clientdata) + SWIG_TypeClientData(equiv->type, swig_module.types[i]->clientdata); + } + equiv = equiv->next; + } + } + } +} + +#ifdef __cplusplus +#if 0 +{ + /* c-mode */ +#endif +} +#endif + + + +#ifdef __cplusplus +extern "C" { +#endif + + /* Python-specific SWIG API */ +#define SWIG_newvarlink() SWIG_Python_newvarlink() +#define SWIG_addvarlink(p, name, get_attr, set_attr) SWIG_Python_addvarlink(p, name, get_attr, set_attr) +#define SWIG_InstallConstants(d, constants) SWIG_Python_InstallConstants(d, constants) + + /* ----------------------------------------------------------------------------- + * global variable support code. + * ----------------------------------------------------------------------------- */ + + typedef struct swig_globalvar { + char *name; /* Name of global variable */ + PyObject *(*get_attr)(void); /* Return the current value */ + int (*set_attr)(PyObject *); /* Set the value */ + struct swig_globalvar *next; + } swig_globalvar; + + typedef struct swig_varlinkobject { + PyObject_HEAD + swig_globalvar *vars; + } swig_varlinkobject; + + SWIGINTERN PyObject * + swig_varlink_repr(swig_varlinkobject *SWIGUNUSEDPARM(v)) { +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_InternFromString(""); +#else + return PyString_FromString(""); +#endif + } + + SWIGINTERN PyObject * + swig_varlink_str(swig_varlinkobject *v) { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *str = PyUnicode_InternFromString("("); + PyObject *tail; + PyObject *joined; + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + tail = PyUnicode_FromString(var->name); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + if (var->next) { + tail = PyUnicode_InternFromString(", "); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + } + } + tail = PyUnicode_InternFromString(")"); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; +#else + PyObject *str = PyString_FromString("("); + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + PyString_ConcatAndDel(&str,PyString_FromString(var->name)); + if (var->next) PyString_ConcatAndDel(&str,PyString_FromString(", ")); + } + PyString_ConcatAndDel(&str,PyString_FromString(")")); +#endif + return str; + } + + SWIGINTERN void + swig_varlink_dealloc(swig_varlinkobject *v) { + swig_globalvar *var = v->vars; + while (var) { + swig_globalvar *n = var->next; + free(var->name); + free(var); + var = n; + } + } + + SWIGINTERN PyObject * + swig_varlink_getattr(swig_varlinkobject *v, char *n) { + PyObject *res = NULL; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->get_attr)(); + break; + } + var = var->next; + } + if (res == NULL && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN int + swig_varlink_setattr(swig_varlinkobject *v, char *n, PyObject *p) { + int res = 1; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->set_attr)(p); + break; + } + var = var->next; + } + if (res == 1 && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN PyTypeObject* + swig_varlink_type(void) { + static char varlink__doc__[] = "Swig var link object"; + static PyTypeObject varlink_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "swigvarlink", /* tp_name */ + sizeof(swig_varlinkobject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor) swig_varlink_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc) swig_varlink_getattr, /* tp_getattr */ + (setattrfunc) swig_varlink_setattr, /* tp_setattr */ + 0, /* tp_compare */ + (reprfunc) swig_varlink_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + 0, /* tp_hash */ + 0, /* tp_call */ + (reprfunc) swig_varlink_str, /* tp_str */ + 0, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + 0, /* tp_flags */ + varlink__doc__, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, /* tp_iter -> tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + varlink_type = tmp; + type_init = 1; + if (PyType_Ready(&varlink_type) < 0) + return NULL; + } + return &varlink_type; + } + + /* Create a variable linking object for use later */ + SWIGINTERN PyObject * + SWIG_Python_newvarlink(void) { + swig_varlinkobject *result = PyObject_NEW(swig_varlinkobject, swig_varlink_type()); + if (result) { + result->vars = 0; + } + return ((PyObject*) result); + } + + SWIGINTERN void + SWIG_Python_addvarlink(PyObject *p, const char *name, PyObject *(*get_attr)(void), int (*set_attr)(PyObject *p)) { + swig_varlinkobject *v = (swig_varlinkobject *) p; + swig_globalvar *gv = (swig_globalvar *) malloc(sizeof(swig_globalvar)); + if (gv) { + size_t size = strlen(name)+1; + gv->name = (char *)malloc(size); + if (gv->name) { + memcpy(gv->name, name, size); + gv->get_attr = get_attr; + gv->set_attr = set_attr; + gv->next = v->vars; + } + } + v->vars = gv; + } + + SWIGINTERN PyObject * + SWIG_globals(void) { + static PyObject *globals = 0; + if (!globals) { + globals = SWIG_newvarlink(); + } + return globals; + } + + /* ----------------------------------------------------------------------------- + * constants/methods manipulation + * ----------------------------------------------------------------------------- */ + + /* Install Constants */ + SWIGINTERN void + SWIG_Python_InstallConstants(PyObject *d, swig_const_info constants[]) { + PyObject *obj = 0; + size_t i; + for (i = 0; constants[i].type; ++i) { + switch(constants[i].type) { + case SWIG_PY_POINTER: + obj = SWIG_InternalNewPointerObj(constants[i].pvalue, *(constants[i]).ptype,0); + break; + case SWIG_PY_BINARY: + obj = SWIG_NewPackedObj(constants[i].pvalue, constants[i].lvalue, *(constants[i].ptype)); + break; + default: + obj = 0; + break; + } + if (obj) { + PyDict_SetItemString(d, constants[i].name, obj); + Py_DECREF(obj); + } + } + } + + /* -----------------------------------------------------------------------------*/ + /* Fix SwigMethods to carry the callback ptrs when needed */ + /* -----------------------------------------------------------------------------*/ + + SWIGINTERN void + SWIG_Python_FixMethods(PyMethodDef *methods, + swig_const_info *const_table, + swig_type_info **types, + swig_type_info **types_initial) { + size_t i; + for (i = 0; methods[i].ml_name; ++i) { + const char *c = methods[i].ml_doc; + if (!c) continue; + c = strstr(c, "swig_ptr: "); + if (c) { + int j; + swig_const_info *ci = 0; + const char *name = c + 10; + for (j = 0; const_table[j].type; ++j) { + if (strncmp(const_table[j].name, name, + strlen(const_table[j].name)) == 0) { + ci = &(const_table[j]); + break; + } + } + if (ci) { + void *ptr = (ci->type == SWIG_PY_POINTER) ? ci->pvalue : 0; + if (ptr) { + size_t shift = (ci->ptype) - types; + swig_type_info *ty = types_initial[shift]; + size_t ldoc = (c - methods[i].ml_doc); + size_t lptr = strlen(ty->name)+2*sizeof(void*)+2; + char *ndoc = (char*)malloc(ldoc + lptr + 10); + if (ndoc) { + char *buff = ndoc; + memcpy(buff, methods[i].ml_doc, ldoc); + buff += ldoc; + memcpy(buff, "swig_ptr: ", 10); + buff += 10; + SWIG_PackVoidPtr(buff, ptr, ty->name, lptr); + methods[i].ml_doc = ndoc; + } + } + } + } + } + } + + /* ----------------------------------------------------------------------------- + * Method creation and docstring support functions + * ----------------------------------------------------------------------------- */ + + /* ----------------------------------------------------------------------------- + * Function to find the method definition with the correct docstring for the + * proxy module as opposed to the low-level API + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name) { + /* Find the function in the modified method table */ + size_t offset = 0; + int found = 0; + while (SwigMethods_proxydocs[offset].ml_meth != NULL) { + if (strcmp(SwigMethods_proxydocs[offset].ml_name, name) == 0) { + found = 1; + break; + } + offset++; + } + /* Use the copy with the modified docstring if available */ + return found ? &SwigMethods_proxydocs[offset] : NULL; + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyInstanceMethod_New() used in Python 3 + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } +#if PY_VERSION_HEX >= 0x03000000 + return PyInstanceMethod_New(func); +#else + return PyMethod_New(func, NULL, NULL); +#endif + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyStaticMethod_New() + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } + return PyStaticMethod_New(func); + } + +#ifdef __cplusplus +} +#endif + +/* -----------------------------------------------------------------------------* + * Partial Init method + * -----------------------------------------------------------------------------*/ + +#ifdef __cplusplus +extern "C" +#endif + +SWIGEXPORT +#if PY_VERSION_HEX >= 0x03000000 +PyObject* +#else +void +#endif +SWIG_init(void) { + PyObject *m, *d, *md, *globals; + +#if PY_VERSION_HEX >= 0x03000000 + static struct PyModuleDef SWIG_module = { + PyModuleDef_HEAD_INIT, + SWIG_name, + NULL, + -1, + SwigMethods, + NULL, + NULL, + NULL, + NULL + }; +#endif + +#if defined(SWIGPYTHON_BUILTIN) + static SwigPyClientData SwigPyObject_clientdata = { + 0, 0, 0, 0, 0, 0, 0 + }; + static PyGetSetDef this_getset_def = { + (char *)"this", &SwigPyBuiltin_ThisClosure, NULL, NULL, NULL + }; + static SwigPyGetSet thisown_getset_closure = { + SwigPyObject_own, + SwigPyObject_own + }; + static PyGetSetDef thisown_getset_def = { + (char *)"thisown", SwigPyBuiltin_GetterClosure, SwigPyBuiltin_SetterClosure, NULL, &thisown_getset_closure + }; + PyTypeObject *builtin_pytype; + int builtin_base_count; + swig_type_info *builtin_basetype; + PyObject *tuple; + PyGetSetDescrObject *static_getset; + PyTypeObject *metatype; + PyTypeObject *swigpyobject; + SwigPyClientData *cd; + PyObject *public_interface, *public_symbol; + PyObject *this_descr; + PyObject *thisown_descr; + PyObject *self = 0; + int i; + + (void)builtin_pytype; + (void)builtin_base_count; + (void)builtin_basetype; + (void)tuple; + (void)static_getset; + (void)self; + + /* Metaclass is used to implement static member variables */ + metatype = SwigPyObjectType(); + assert(metatype); +#endif + + (void)globals; + + /* Create singletons now to avoid potential deadlocks with multi-threaded usage after module initialization */ + SWIG_This(); + SWIG_Python_TypeCache(); + SwigPyPacked_type(); +#ifndef SWIGPYTHON_BUILTIN + SwigPyObject_type(); +#endif + + /* Fix SwigMethods to carry the callback ptrs when needed */ + SWIG_Python_FixMethods(SwigMethods, swig_const_table, swig_types, swig_type_initial); + +#if PY_VERSION_HEX >= 0x03000000 + m = PyModule_Create(&SWIG_module); +#else + m = Py_InitModule(SWIG_name, SwigMethods); +#endif + + md = d = PyModule_GetDict(m); + (void)md; + + SWIG_InitializeModule(0); + +#ifdef SWIGPYTHON_BUILTIN + swigpyobject = SwigPyObject_TypeOnce(); + + SwigPyObject_stype = SWIG_MangledTypeQuery("_p_SwigPyObject"); + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + if (!cd) { + SwigPyObject_stype->clientdata = &SwigPyObject_clientdata; + SwigPyObject_clientdata.pytype = swigpyobject; + } else if (swigpyobject->tp_basicsize != cd->pytype->tp_basicsize) { + PyErr_SetString(PyExc_RuntimeError, "Import error: attempted to load two incompatible swig-generated modules."); +# if PY_VERSION_HEX >= 0x03000000 + return NULL; +# else + return; +# endif + } + + /* All objects have a 'this' attribute */ + this_descr = PyDescr_NewGetSet(SwigPyObject_type(), &this_getset_def); + (void)this_descr; + + /* All objects have a 'thisown' attribute */ + thisown_descr = PyDescr_NewGetSet(SwigPyObject_type(), &thisown_getset_def); + (void)thisown_descr; + + public_interface = PyList_New(0); + public_symbol = 0; + (void)public_symbol; + + PyDict_SetItemString(md, "__all__", public_interface); + Py_DECREF(public_interface); + for (i = 0; SwigMethods[i].ml_name != NULL; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, SwigMethods[i].ml_name); + for (i = 0; swig_const_table[i].name != 0; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, swig_const_table[i].name); +#endif + + SWIG_InstallConstants(d,swig_const_table); + + SWIG_Python_SetConstant(d, "Driver_INDICATOR_OFF",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_OFF))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_RIGHT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_RIGHT))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_LEFT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_LEFT))); + SWIG_Python_SetConstant(d, "Driver_SPEED",SWIG_From_int(static_cast< int >(webots::Driver::SPEED))); + SWIG_Python_SetConstant(d, "Driver_TORQUE",SWIG_From_int(static_cast< int >(webots::Driver::TORQUE))); + SWIG_Python_SetConstant(d, "Driver_DOWN",SWIG_From_int(static_cast< int >(webots::Driver::DOWN))); + SWIG_Python_SetConstant(d, "Driver_SLOW",SWIG_From_int(static_cast< int >(webots::Driver::SLOW))); + SWIG_Python_SetConstant(d, "Driver_NORMAL",SWIG_From_int(static_cast< int >(webots::Driver::NORMAL))); + SWIG_Python_SetConstant(d, "Driver_FAST",SWIG_From_int(static_cast< int >(webots::Driver::FAST))); + SWIG_Python_SetConstant(d, "Car_TRACTION",SWIG_From_int(static_cast< int >(webots::Car::TRACTION))); + SWIG_Python_SetConstant(d, "Car_PROPULSION",SWIG_From_int(static_cast< int >(webots::Car::PROPULSION))); + SWIG_Python_SetConstant(d, "Car_FOUR_BY_FOUR",SWIG_From_int(static_cast< int >(webots::Car::FOUR_BY_FOUR))); + SWIG_Python_SetConstant(d, "Car_COMBUSTION_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::COMBUSTION_ENGINE))); + SWIG_Python_SetConstant(d, "Car_ELECTRIC_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::ELECTRIC_ENGINE))); + SWIG_Python_SetConstant(d, "Car_PARALLEL_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::PARALLEL_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_POWER_SPLIT_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::POWER_SPLIT_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_NB",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_NB))); +#if PY_VERSION_HEX >= 0x03000000 + return m; +#else + return; +#endif +} + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle37.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle37.cpp new file mode 100644 index 000000000..cbfd22412 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle37.cpp @@ -0,0 +1,5255 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 4.0.2 + * + * This file is not intended to be easily readable and contains a number of + * coding conventions designed to improve portability and efficiency. Do not make + * changes to this file unless you know what you are doing--modify the SWIG + * interface file instead. + * ----------------------------------------------------------------------------- */ + + +#ifndef SWIGPYTHON +#define SWIGPYTHON +#endif + +#define SWIG_PYTHON_DIRECTOR_NO_VTABLE + + +#ifdef __cplusplus +/* SwigValueWrapper is described in swig.swg */ +template class SwigValueWrapper { + struct SwigMovePointer { + T *ptr; + SwigMovePointer(T *p) : ptr(p) { } + ~SwigMovePointer() { delete ptr; } + SwigMovePointer& operator=(SwigMovePointer& rhs) { T* oldptr = ptr; ptr = 0; delete oldptr; ptr = rhs.ptr; rhs.ptr = 0; return *this; } + } pointer; + SwigValueWrapper& operator=(const SwigValueWrapper& rhs); + SwigValueWrapper(const SwigValueWrapper& rhs); +public: + SwigValueWrapper() : pointer(0) { } + SwigValueWrapper& operator=(const T& t) { SwigMovePointer tmp(new T(t)); pointer = tmp; return *this; } + operator T&() const { return *pointer.ptr; } + T *operator&() { return pointer.ptr; } +}; + +template T SwigValueInit() { + return T(); +} +#endif + +/* ----------------------------------------------------------------------------- + * This section contains generic SWIG labels for method/variable + * declarations/attributes, and other compiler dependent labels. + * ----------------------------------------------------------------------------- */ + +/* template workaround for compilers that cannot correctly implement the C++ standard */ +#ifndef SWIGTEMPLATEDISAMBIGUATOR +# if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x560) +# define SWIGTEMPLATEDISAMBIGUATOR template +# elif defined(__HP_aCC) +/* Needed even with `aCC -AA' when `aCC -V' reports HP ANSI C++ B3910B A.03.55 */ +/* If we find a maximum version that requires this, the test would be __HP_aCC <= 35500 for A.03.55 */ +# define SWIGTEMPLATEDISAMBIGUATOR template +# else +# define SWIGTEMPLATEDISAMBIGUATOR +# endif +#endif + +/* inline attribute */ +#ifndef SWIGINLINE +# if defined(__cplusplus) || (defined(__GNUC__) && !defined(__STRICT_ANSI__)) +# define SWIGINLINE inline +# else +# define SWIGINLINE +# endif +#endif + +/* attribute recognised by some compilers to avoid 'unused' warnings */ +#ifndef SWIGUNUSED +# if defined(__GNUC__) +# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +# elif defined(__ICC) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +#endif + +#ifndef SWIG_MSC_UNSUPPRESS_4505 +# if defined(_MSC_VER) +# pragma warning(disable : 4505) /* unreferenced local function has been removed */ +# endif +#endif + +#ifndef SWIGUNUSEDPARM +# ifdef __cplusplus +# define SWIGUNUSEDPARM(p) +# else +# define SWIGUNUSEDPARM(p) p SWIGUNUSED +# endif +#endif + +/* internal SWIG method */ +#ifndef SWIGINTERN +# define SWIGINTERN static SWIGUNUSED +#endif + +/* internal inline SWIG method */ +#ifndef SWIGINTERNINLINE +# define SWIGINTERNINLINE SWIGINTERN SWIGINLINE +#endif + +/* exporting methods */ +#if defined(__GNUC__) +# if (__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) +# ifndef GCC_HASCLASSVISIBILITY +# define GCC_HASCLASSVISIBILITY +# endif +# endif +#endif + +#ifndef SWIGEXPORT +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# if defined(STATIC_LINKED) +# define SWIGEXPORT +# else +# define SWIGEXPORT __declspec(dllexport) +# endif +# else +# if defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) +# define SWIGEXPORT __attribute__ ((visibility("default"))) +# else +# define SWIGEXPORT +# endif +# endif +#endif + +/* calling conventions for Windows */ +#ifndef SWIGSTDCALL +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# define SWIGSTDCALL __stdcall +# else +# define SWIGSTDCALL +# endif +#endif + +/* Deal with Microsoft's attempt at deprecating C standard runtime functions */ +#if !defined(SWIG_NO_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_CRT_SECURE_NO_DEPRECATE) +# define _CRT_SECURE_NO_DEPRECATE +#endif + +/* Deal with Microsoft's attempt at deprecating methods in the standard C++ library */ +#if !defined(SWIG_NO_SCL_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_SCL_SECURE_NO_DEPRECATE) +# define _SCL_SECURE_NO_DEPRECATE +#endif + +/* Deal with Apple's deprecated 'AssertMacros.h' from Carbon-framework */ +#if defined(__APPLE__) && !defined(__ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES) +# define __ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES 0 +#endif + +/* Intel's compiler complains if a variable which was never initialised is + * cast to void, which is a common idiom which we use to indicate that we + * are aware a variable isn't used. So we just silence that warning. + * See: https://github.com/swig/swig/issues/192 for more discussion. + */ +#ifdef __INTEL_COMPILER +# pragma warning disable 592 +#endif + + +#if defined(__GNUC__) && defined(_WIN32) && !defined(SWIG_PYTHON_NO_HYPOT_WORKAROUND) +/* Workaround for '::hypot' has not been declared', see https://bugs.python.org/issue11566 */ +# include +#endif + +#if defined(_DEBUG) && defined(SWIG_PYTHON_INTERPRETER_NO_DEBUG) +/* Use debug wrappers with the Python release dll */ +# undef _DEBUG +# include +# define _DEBUG 1 +#else +# include +#endif + +/* ----------------------------------------------------------------------------- + * swigrun.swg + * + * This file contains generic C API SWIG runtime support for pointer + * type checking. + * ----------------------------------------------------------------------------- */ + +/* This should only be incremented when either the layout of swig_type_info changes, + or for whatever reason, the runtime changes incompatibly */ +#define SWIG_RUNTIME_VERSION "4" + +/* define SWIG_TYPE_TABLE_NAME as "SWIG_TYPE_TABLE" */ +#ifdef SWIG_TYPE_TABLE +# define SWIG_QUOTE_STRING(x) #x +# define SWIG_EXPAND_AND_QUOTE_STRING(x) SWIG_QUOTE_STRING(x) +# define SWIG_TYPE_TABLE_NAME SWIG_EXPAND_AND_QUOTE_STRING(SWIG_TYPE_TABLE) +#else +# define SWIG_TYPE_TABLE_NAME +#endif + +/* + You can use the SWIGRUNTIME and SWIGRUNTIMEINLINE macros for + creating a static or dynamic library from the SWIG runtime code. + In 99.9% of the cases, SWIG just needs to declare them as 'static'. + + But only do this if strictly necessary, ie, if you have problems + with your compiler or suchlike. +*/ + +#ifndef SWIGRUNTIME +# define SWIGRUNTIME SWIGINTERN +#endif + +#ifndef SWIGRUNTIMEINLINE +# define SWIGRUNTIMEINLINE SWIGRUNTIME SWIGINLINE +#endif + +/* Generic buffer size */ +#ifndef SWIG_BUFFER_SIZE +# define SWIG_BUFFER_SIZE 1024 +#endif + +/* Flags for pointer conversions */ +#define SWIG_POINTER_DISOWN 0x1 +#define SWIG_CAST_NEW_MEMORY 0x2 +#define SWIG_POINTER_NO_NULL 0x4 + +/* Flags for new pointer objects */ +#define SWIG_POINTER_OWN 0x1 + + +/* + Flags/methods for returning states. + + The SWIG conversion methods, as ConvertPtr, return an integer + that tells if the conversion was successful or not. And if not, + an error code can be returned (see swigerrors.swg for the codes). + + Use the following macros/flags to set or process the returning + states. + + In old versions of SWIG, code such as the following was usually written: + + if (SWIG_ConvertPtr(obj,vptr,ty.flags) != -1) { + // success code + } else { + //fail code + } + + Now you can be more explicit: + + int res = SWIG_ConvertPtr(obj,vptr,ty.flags); + if (SWIG_IsOK(res)) { + // success code + } else { + // fail code + } + + which is the same really, but now you can also do + + Type *ptr; + int res = SWIG_ConvertPtr(obj,(void **)(&ptr),ty.flags); + if (SWIG_IsOK(res)) { + // success code + if (SWIG_IsNewObj(res) { + ... + delete *ptr; + } else { + ... + } + } else { + // fail code + } + + I.e., now SWIG_ConvertPtr can return new objects and you can + identify the case and take care of the deallocation. Of course that + also requires SWIG_ConvertPtr to return new result values, such as + + int SWIG_ConvertPtr(obj, ptr,...) { + if () { + if () { + *ptr = ; + return SWIG_NEWOBJ; + } else { + *ptr = ; + return SWIG_OLDOBJ; + } + } else { + return SWIG_BADOBJ; + } + } + + Of course, returning the plain '0(success)/-1(fail)' still works, but you can be + more explicit by returning SWIG_BADOBJ, SWIG_ERROR or any of the + SWIG errors code. + + Finally, if the SWIG_CASTRANK_MODE is enabled, the result code + allows to return the 'cast rank', for example, if you have this + + int food(double) + int fooi(int); + + and you call + + food(1) // cast rank '1' (1 -> 1.0) + fooi(1) // cast rank '0' + + just use the SWIG_AddCast()/SWIG_CheckState() +*/ + +#define SWIG_OK (0) +#define SWIG_ERROR (-1) +#define SWIG_IsOK(r) (r >= 0) +#define SWIG_ArgError(r) ((r != SWIG_ERROR) ? r : SWIG_TypeError) + +/* The CastRankLimit says how many bits are used for the cast rank */ +#define SWIG_CASTRANKLIMIT (1 << 8) +/* The NewMask denotes the object was created (using new/malloc) */ +#define SWIG_NEWOBJMASK (SWIG_CASTRANKLIMIT << 1) +/* The TmpMask is for in/out typemaps that use temporal objects */ +#define SWIG_TMPOBJMASK (SWIG_NEWOBJMASK << 1) +/* Simple returning values */ +#define SWIG_BADOBJ (SWIG_ERROR) +#define SWIG_OLDOBJ (SWIG_OK) +#define SWIG_NEWOBJ (SWIG_OK | SWIG_NEWOBJMASK) +#define SWIG_TMPOBJ (SWIG_OK | SWIG_TMPOBJMASK) +/* Check, add and del mask methods */ +#define SWIG_AddNewMask(r) (SWIG_IsOK(r) ? (r | SWIG_NEWOBJMASK) : r) +#define SWIG_DelNewMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_NEWOBJMASK) : r) +#define SWIG_IsNewObj(r) (SWIG_IsOK(r) && (r & SWIG_NEWOBJMASK)) +#define SWIG_AddTmpMask(r) (SWIG_IsOK(r) ? (r | SWIG_TMPOBJMASK) : r) +#define SWIG_DelTmpMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_TMPOBJMASK) : r) +#define SWIG_IsTmpObj(r) (SWIG_IsOK(r) && (r & SWIG_TMPOBJMASK)) + +/* Cast-Rank Mode */ +#if defined(SWIG_CASTRANK_MODE) +# ifndef SWIG_TypeRank +# define SWIG_TypeRank unsigned long +# endif +# ifndef SWIG_MAXCASTRANK /* Default cast allowed */ +# define SWIG_MAXCASTRANK (2) +# endif +# define SWIG_CASTRANKMASK ((SWIG_CASTRANKLIMIT) -1) +# define SWIG_CastRank(r) (r & SWIG_CASTRANKMASK) +SWIGINTERNINLINE int SWIG_AddCast(int r) { + return SWIG_IsOK(r) ? ((SWIG_CastRank(r) < SWIG_MAXCASTRANK) ? (r + 1) : SWIG_ERROR) : r; +} +SWIGINTERNINLINE int SWIG_CheckState(int r) { + return SWIG_IsOK(r) ? SWIG_CastRank(r) + 1 : 0; +} +#else /* no cast-rank mode */ +# define SWIG_AddCast(r) (r) +# define SWIG_CheckState(r) (SWIG_IsOK(r) ? 1 : 0) +#endif + + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void *(*swig_converter_func)(void *, int *); +typedef struct swig_type_info *(*swig_dycast_func)(void **); + +/* Structure to store information on one type */ +typedef struct swig_type_info { + const char *name; /* mangled name of this type */ + const char *str; /* human readable name of this type */ + swig_dycast_func dcast; /* dynamic cast function down a hierarchy */ + struct swig_cast_info *cast; /* linked list of types that can cast into this type */ + void *clientdata; /* language specific type data */ + int owndata; /* flag if the structure owns the clientdata */ +} swig_type_info; + +/* Structure to store a type and conversion function used for casting */ +typedef struct swig_cast_info { + swig_type_info *type; /* pointer to type that is equivalent to this type */ + swig_converter_func converter; /* function to cast the void pointers */ + struct swig_cast_info *next; /* pointer to next cast in linked list */ + struct swig_cast_info *prev; /* pointer to the previous cast */ +} swig_cast_info; + +/* Structure used to store module information + * Each module generates one structure like this, and the runtime collects + * all of these structures and stores them in a circularly linked list.*/ +typedef struct swig_module_info { + swig_type_info **types; /* Array of pointers to swig_type_info structures that are in this module */ + size_t size; /* Number of types in this module */ + struct swig_module_info *next; /* Pointer to next element in circularly linked list */ + swig_type_info **type_initial; /* Array of initially generated type structures */ + swig_cast_info **cast_initial; /* Array of initially generated casting structures */ + void *clientdata; /* Language specific module data */ +} swig_module_info; + +/* + Compare two type names skipping the space characters, therefore + "char*" == "char *" and "Class" == "Class", etc. + + Return 0 when the two name types are equivalent, as in + strncmp, but skipping ' '. +*/ +SWIGRUNTIME int +SWIG_TypeNameComp(const char *f1, const char *l1, + const char *f2, const char *l2) { + for (;(f1 != l1) && (f2 != l2); ++f1, ++f2) { + while ((*f1 == ' ') && (f1 != l1)) ++f1; + while ((*f2 == ' ') && (f2 != l2)) ++f2; + if (*f1 != *f2) return (*f1 > *f2) ? 1 : -1; + } + return (int)((l1 - f1) - (l2 - f2)); +} + +/* + Check type equivalence in a name list like ||... + Return 0 if equal, -1 if nb < tb, 1 if nb > tb +*/ +SWIGRUNTIME int +SWIG_TypeCmp(const char *nb, const char *tb) { + int equiv = 1; + const char* te = tb + strlen(tb); + const char* ne = nb; + while (equiv != 0 && *ne) { + for (nb = ne; *ne; ++ne) { + if (*ne == '|') break; + } + equiv = SWIG_TypeNameComp(nb, ne, tb, te); + if (*ne) ++ne; + } + return equiv; +} + +/* + Check type equivalence in a name list like ||... + Return 0 if not equal, 1 if equal +*/ +SWIGRUNTIME int +SWIG_TypeEquiv(const char *nb, const char *tb) { + return SWIG_TypeCmp(nb, tb) == 0 ? 1 : 0; +} + +/* + Check the typename +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheck(const char *c, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (strcmp(iter->type->name, c) == 0) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Identical to SWIG_TypeCheck, except strcmp is replaced with a pointer comparison +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheckStruct(swig_type_info *from, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (iter->type == from) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Cast a pointer up an inheritance hierarchy +*/ +SWIGRUNTIMEINLINE void * +SWIG_TypeCast(swig_cast_info *ty, void *ptr, int *newmemory) { + return ((!ty) || (!ty->converter)) ? ptr : (*ty->converter)(ptr, newmemory); +} + +/* + Dynamic pointer casting. Down an inheritance hierarchy +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeDynamicCast(swig_type_info *ty, void **ptr) { + swig_type_info *lastty = ty; + if (!ty || !ty->dcast) return ty; + while (ty && (ty->dcast)) { + ty = (*ty->dcast)(ptr); + if (ty) lastty = ty; + } + return lastty; +} + +/* + Return the name associated with this type +*/ +SWIGRUNTIMEINLINE const char * +SWIG_TypeName(const swig_type_info *ty) { + return ty->name; +} + +/* + Return the pretty name associated with this type, + that is an unmangled type name in a form presentable to the user. +*/ +SWIGRUNTIME const char * +SWIG_TypePrettyName(const swig_type_info *type) { + /* The "str" field contains the equivalent pretty names of the + type, separated by vertical-bar characters. We choose + to print the last name, as it is often (?) the most + specific. */ + if (!type) return NULL; + if (type->str != NULL) { + const char *last_name = type->str; + const char *s; + for (s = type->str; *s; s++) + if (*s == '|') last_name = s+1; + return last_name; + } + else + return type->name; +} + +/* + Set the clientdata field for a type +*/ +SWIGRUNTIME void +SWIG_TypeClientData(swig_type_info *ti, void *clientdata) { + swig_cast_info *cast = ti->cast; + /* if (ti->clientdata == clientdata) return; */ + ti->clientdata = clientdata; + + while (cast) { + if (!cast->converter) { + swig_type_info *tc = cast->type; + if (!tc->clientdata) { + SWIG_TypeClientData(tc, clientdata); + } + } + cast = cast->next; + } +} +SWIGRUNTIME void +SWIG_TypeNewClientData(swig_type_info *ti, void *clientdata) { + SWIG_TypeClientData(ti, clientdata); + ti->owndata = 1; +} + +/* + Search for a swig_type_info structure only by mangled name + Search is a O(log #types) + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_MangledTypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + swig_module_info *iter = start; + do { + if (iter->size) { + size_t l = 0; + size_t r = iter->size - 1; + do { + /* since l+r >= 0, we can (>> 1) instead (/ 2) */ + size_t i = (l + r) >> 1; + const char *iname = iter->types[i]->name; + if (iname) { + int compare = strcmp(name, iname); + if (compare == 0) { + return iter->types[i]; + } else if (compare < 0) { + if (i) { + r = i - 1; + } else { + break; + } + } else if (compare > 0) { + l = i + 1; + } + } else { + break; /* should never happen */ + } + } while (l <= r); + } + iter = iter->next; + } while (iter != end); + return 0; +} + +/* + Search for a swig_type_info structure for either a mangled name or a human readable name. + It first searches the mangled names of the types, which is a O(log #types) + If a type is not found it then searches the human readable names, which is O(#types). + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + /* STEP 1: Search the name field using binary search */ + swig_type_info *ret = SWIG_MangledTypeQueryModule(start, end, name); + if (ret) { + return ret; + } else { + /* STEP 2: If the type hasn't been found, do a complete search + of the str field (the human readable name) */ + swig_module_info *iter = start; + do { + size_t i = 0; + for (; i < iter->size; ++i) { + if (iter->types[i]->str && (SWIG_TypeEquiv(iter->types[i]->str, name))) + return iter->types[i]; + } + iter = iter->next; + } while (iter != end); + } + + /* neither found a match */ + return 0; +} + +/* + Pack binary data into a string +*/ +SWIGRUNTIME char * +SWIG_PackData(char *c, void *ptr, size_t sz) { + static const char hex[17] = "0123456789abcdef"; + const unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + unsigned char uu = *u; + *(c++) = hex[(uu & 0xf0) >> 4]; + *(c++) = hex[uu & 0xf]; + } + return c; +} + +/* + Unpack binary data from a string +*/ +SWIGRUNTIME const char * +SWIG_UnpackData(const char *c, void *ptr, size_t sz) { + unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + char d = *(c++); + unsigned char uu; + if ((d >= '0') && (d <= '9')) + uu = (unsigned char)((d - '0') << 4); + else if ((d >= 'a') && (d <= 'f')) + uu = (unsigned char)((d - ('a'-10)) << 4); + else + return (char *) 0; + d = *(c++); + if ((d >= '0') && (d <= '9')) + uu |= (unsigned char)(d - '0'); + else if ((d >= 'a') && (d <= 'f')) + uu |= (unsigned char)(d - ('a'-10)); + else + return (char *) 0; + *u = uu; + } + return c; +} + +/* + Pack 'void *' into a string buffer. +*/ +SWIGRUNTIME char * +SWIG_PackVoidPtr(char *buff, void *ptr, const char *name, size_t bsz) { + char *r = buff; + if ((2*sizeof(void *) + 2) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,&ptr,sizeof(void *)); + if (strlen(name) + 1 > (bsz - (r - buff))) return 0; + strcpy(r,name); + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackVoidPtr(const char *c, void **ptr, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + *ptr = (void *) 0; + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sizeof(void *)); +} + +SWIGRUNTIME char * +SWIG_PackDataName(char *buff, void *ptr, size_t sz, const char *name, size_t bsz) { + char *r = buff; + size_t lname = (name ? strlen(name) : 0); + if ((2*sz + 2 + lname) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,ptr,sz); + if (lname) { + strncpy(r,name,lname+1); + } else { + *r = 0; + } + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackDataName(const char *c, void *ptr, size_t sz, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + memset(ptr,0,sz); + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sz); +} + +#ifdef __cplusplus +} +#endif + +/* Errors in SWIG */ +#define SWIG_UnknownError -1 +#define SWIG_IOError -2 +#define SWIG_RuntimeError -3 +#define SWIG_IndexError -4 +#define SWIG_TypeError -5 +#define SWIG_DivisionByZero -6 +#define SWIG_OverflowError -7 +#define SWIG_SyntaxError -8 +#define SWIG_ValueError -9 +#define SWIG_SystemError -10 +#define SWIG_AttributeError -11 +#define SWIG_MemoryError -12 +#define SWIG_NullReferenceError -13 + + + +/* Compatibility macros for Python 3 */ +#if PY_VERSION_HEX >= 0x03000000 + +#define PyClass_Check(obj) PyObject_IsInstance(obj, (PyObject *)&PyType_Type) +#define PyInt_Check(x) PyLong_Check(x) +#define PyInt_AsLong(x) PyLong_AsLong(x) +#define PyInt_FromLong(x) PyLong_FromLong(x) +#define PyInt_FromSize_t(x) PyLong_FromSize_t(x) +#define PyString_Check(name) PyBytes_Check(name) +#define PyString_FromString(x) PyUnicode_FromString(x) +#define PyString_Format(fmt, args) PyUnicode_Format(fmt, args) +#define PyString_AsString(str) PyBytes_AsString(str) +#define PyString_Size(str) PyBytes_Size(str) +#define PyString_InternFromString(key) PyUnicode_InternFromString(key) +#define Py_TPFLAGS_HAVE_CLASS Py_TPFLAGS_BASETYPE +#define PyString_AS_STRING(x) PyUnicode_AS_STRING(x) +#define _PyLong_FromSsize_t(x) PyLong_FromSsize_t(x) + +#endif + +#ifndef Py_TYPE +# define Py_TYPE(op) ((op)->ob_type) +#endif + +/* SWIG APIs for compatibility of both Python 2 & 3 */ + +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_Python_str_FromFormat PyUnicode_FromFormat +#else +# define SWIG_Python_str_FromFormat PyString_FromFormat +#endif + + +/* Warning: This function will allocate a new string in Python 3, + * so please call SWIG_Python_str_DelForPy3(x) to free the space. + */ +SWIGINTERN char* +SWIG_Python_str_AsChar(PyObject *str) +{ +#if PY_VERSION_HEX >= 0x03030000 + return (char *)PyUnicode_AsUTF8(str); +#elif PY_VERSION_HEX >= 0x03000000 + char *newstr = 0; + str = PyUnicode_AsUTF8String(str); + if (str) { + char *cstr; + Py_ssize_t len; + if (PyBytes_AsStringAndSize(str, &cstr, &len) != -1) { + newstr = (char *) malloc(len+1); + if (newstr) + memcpy(newstr, cstr, len+1); + } + Py_XDECREF(str); + } + return newstr; +#else + return PyString_AsString(str); +#endif +} + +#if PY_VERSION_HEX >= 0x03030000 || PY_VERSION_HEX < 0x03000000 +# define SWIG_Python_str_DelForPy3(x) +#else +# define SWIG_Python_str_DelForPy3(x) free( (void*) (x) ) +#endif + + +SWIGINTERN PyObject* +SWIG_Python_str_FromChar(const char *c) +{ +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_FromString(c); +#else + return PyString_FromString(c); +#endif +} + +#ifndef PyObject_DEL +# define PyObject_DEL PyObject_Del +#endif + +// SWIGPY_USE_CAPSULE is no longer used within SWIG itself, but some user +// interface files check for it. +# define SWIGPY_USE_CAPSULE +# define SWIGPY_CAPSULE_NAME ("swig_runtime_data" SWIG_RUNTIME_VERSION ".type_pointer_capsule" SWIG_TYPE_TABLE_NAME) + +#if PY_VERSION_HEX < 0x03020000 +#define PyDescr_TYPE(x) (((PyDescrObject *)(x))->d_type) +#define PyDescr_NAME(x) (((PyDescrObject *)(x))->d_name) +#define Py_hash_t long +#endif + +/* ----------------------------------------------------------------------------- + * error manipulation + * ----------------------------------------------------------------------------- */ + +SWIGRUNTIME PyObject* +SWIG_Python_ErrorType(int code) { + PyObject* type = 0; + switch(code) { + case SWIG_MemoryError: + type = PyExc_MemoryError; + break; + case SWIG_IOError: + type = PyExc_IOError; + break; + case SWIG_RuntimeError: + type = PyExc_RuntimeError; + break; + case SWIG_IndexError: + type = PyExc_IndexError; + break; + case SWIG_TypeError: + type = PyExc_TypeError; + break; + case SWIG_DivisionByZero: + type = PyExc_ZeroDivisionError; + break; + case SWIG_OverflowError: + type = PyExc_OverflowError; + break; + case SWIG_SyntaxError: + type = PyExc_SyntaxError; + break; + case SWIG_ValueError: + type = PyExc_ValueError; + break; + case SWIG_SystemError: + type = PyExc_SystemError; + break; + case SWIG_AttributeError: + type = PyExc_AttributeError; + break; + default: + type = PyExc_RuntimeError; + } + return type; +} + + +SWIGRUNTIME void +SWIG_Python_AddErrorMsg(const char* mesg) +{ + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + + if (PyErr_Occurred()) + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + PyErr_Clear(); + Py_XINCREF(type); + if (tmp) + PyErr_Format(type, "%s %s", tmp, mesg); + else + PyErr_Format(type, "%s", mesg); + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + Py_DECREF(value); + } else { + PyErr_SetString(PyExc_RuntimeError, mesg); + } +} + +SWIGRUNTIME int +SWIG_Python_TypeErrorOccurred(PyObject *obj) +{ + PyObject *error; + if (obj) + return 0; + error = PyErr_Occurred(); + return error && PyErr_GivenExceptionMatches(error, PyExc_TypeError); +} + +SWIGRUNTIME void +SWIG_Python_RaiseOrModifyTypeError(const char *message) +{ + if (SWIG_Python_TypeErrorOccurred(NULL)) { + /* Use existing TypeError to preserve stacktrace and enhance with given message */ + PyObject *newvalue; + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); +#if PY_VERSION_HEX >= 0x03000000 + newvalue = PyUnicode_FromFormat("%S\nAdditional information:\n%s", value, message); +#else + newvalue = PyString_FromFormat("%s\nAdditional information:\n%s", PyString_AsString(value), message); +#endif + Py_XDECREF(value); + PyErr_Restore(type, newvalue, traceback); + } else { + /* Raise TypeError using given message */ + PyErr_SetString(PyExc_TypeError, message); + } +} + +#if defined(SWIG_PYTHON_NO_THREADS) +# if defined(SWIG_PYTHON_THREADS) +# undef SWIG_PYTHON_THREADS +# endif +#endif +#if defined(SWIG_PYTHON_THREADS) /* Threading support is enabled */ +# if !defined(SWIG_PYTHON_USE_GIL) && !defined(SWIG_PYTHON_NO_USE_GIL) +# define SWIG_PYTHON_USE_GIL +# endif +# if defined(SWIG_PYTHON_USE_GIL) /* Use PyGILState threads calls */ +# ifndef SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_INITIALIZE_THREADS PyEval_InitThreads() +# endif +# ifdef __cplusplus /* C++ code */ + class SWIG_Python_Thread_Block { + bool status; + PyGILState_STATE state; + public: + void end() { if (status) { PyGILState_Release(state); status = false;} } + SWIG_Python_Thread_Block() : status(true), state(PyGILState_Ensure()) {} + ~SWIG_Python_Thread_Block() { end(); } + }; + class SWIG_Python_Thread_Allow { + bool status; + PyThreadState *save; + public: + void end() { if (status) { PyEval_RestoreThread(save); status = false; }} + SWIG_Python_Thread_Allow() : status(true), save(PyEval_SaveThread()) {} + ~SWIG_Python_Thread_Allow() { end(); } + }; +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK SWIG_Python_Thread_Block _swig_thread_block +# define SWIG_PYTHON_THREAD_END_BLOCK _swig_thread_block.end() +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW SWIG_Python_Thread_Allow _swig_thread_allow +# define SWIG_PYTHON_THREAD_END_ALLOW _swig_thread_allow.end() +# else /* C code */ +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK PyGILState_STATE _swig_thread_block = PyGILState_Ensure() +# define SWIG_PYTHON_THREAD_END_BLOCK PyGILState_Release(_swig_thread_block) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW PyThreadState *_swig_thread_allow = PyEval_SaveThread() +# define SWIG_PYTHON_THREAD_END_ALLOW PyEval_RestoreThread(_swig_thread_allow) +# endif +# else /* Old thread way, not implemented, user must provide it */ +# if !defined(SWIG_PYTHON_INITIALIZE_THREADS) +# define SWIG_PYTHON_INITIALIZE_THREADS +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_BLOCK) +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_END_BLOCK) +# define SWIG_PYTHON_THREAD_END_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_ALLOW) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# endif +# if !defined(SWIG_PYTHON_THREAD_END_ALLOW) +# define SWIG_PYTHON_THREAD_END_ALLOW +# endif +# endif +#else /* No thread support */ +# define SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# define SWIG_PYTHON_THREAD_END_BLOCK +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# define SWIG_PYTHON_THREAD_END_ALLOW +#endif + +/* ----------------------------------------------------------------------------- + * Python API portion that goes into the runtime + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* ----------------------------------------------------------------------------- + * Constant declarations + * ----------------------------------------------------------------------------- */ + +/* Constant Types */ +#define SWIG_PY_POINTER 4 +#define SWIG_PY_BINARY 5 + +/* Constant information structure */ +typedef struct swig_const_info { + int type; + const char *name; + long lvalue; + double dvalue; + void *pvalue; + swig_type_info **ptype; +} swig_const_info; + +#ifdef __cplusplus +} +#endif + + +/* ----------------------------------------------------------------------------- + * pyrun.swg + * + * This file contains the runtime support for Python modules + * and includes code for managing global variables and pointer + * type checking. + * + * ----------------------------------------------------------------------------- */ + +#if PY_VERSION_HEX < 0x02070000 /* 2.7.0 */ +# error "This version of SWIG only supports Python >= 2.7" +#endif + +#if PY_VERSION_HEX >= 0x03000000 && PY_VERSION_HEX < 0x03020000 +# error "This version of SWIG only supports Python 3 >= 3.2" +#endif + +/* Common SWIG API */ + +/* for raw pointers */ +#define SWIG_Python_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, 0) +#define SWIG_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtr(obj, pptr, type, flags) +#define SWIG_ConvertPtrAndOwn(obj,pptr,type,flags,own) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, own) + +#ifdef SWIGPYTHON_BUILTIN +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(self, ptr, type, flags) +#else +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) +#endif + +#define SWIG_InternalNewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) + +#define SWIG_CheckImplicit(ty) SWIG_Python_CheckImplicit(ty) +#define SWIG_AcquirePtr(ptr, src) SWIG_Python_AcquirePtr(ptr, src) +#define swig_owntype int + +/* for raw packed data */ +#define SWIG_ConvertPacked(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewPackedObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + +/* for class or struct pointers */ +#define SWIG_ConvertInstance(obj, pptr, type, flags) SWIG_ConvertPtr(obj, pptr, type, flags) +#define SWIG_NewInstanceObj(ptr, type, flags) SWIG_NewPointerObj(ptr, type, flags) + +/* for C or C++ function pointers */ +#define SWIG_ConvertFunctionPtr(obj, pptr, type) SWIG_Python_ConvertFunctionPtr(obj, pptr, type) +#define SWIG_NewFunctionPtrObj(ptr, type) SWIG_Python_NewPointerObj(NULL, ptr, type, 0) + +/* for C++ member pointers, ie, member methods */ +#define SWIG_ConvertMember(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewMemberObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + + +/* Runtime API */ + +#define SWIG_GetModule(clientdata) SWIG_Python_GetModule(clientdata) +#define SWIG_SetModule(clientdata, pointer) SWIG_Python_SetModule(pointer) +#define SWIG_NewClientData(obj) SwigPyClientData_New(obj) + +#define SWIG_SetErrorObj SWIG_Python_SetErrorObj +#define SWIG_SetErrorMsg SWIG_Python_SetErrorMsg +#define SWIG_ErrorType(code) SWIG_Python_ErrorType(code) +#define SWIG_Error(code, msg) SWIG_Python_SetErrorMsg(SWIG_ErrorType(code), msg) +#define SWIG_fail goto fail + + +/* Runtime API implementation */ + +/* Error manipulation */ + +SWIGINTERN void +SWIG_Python_SetErrorObj(PyObject *errtype, PyObject *obj) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetObject(errtype, obj); + Py_DECREF(obj); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +SWIGINTERN void +SWIG_Python_SetErrorMsg(PyObject *errtype, const char *msg) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetString(errtype, msg); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +#define SWIG_Python_Raise(obj, type, desc) SWIG_Python_SetErrorObj(SWIG_Python_ExceptionType(desc), obj) + +/* Set a constant value */ + +#if defined(SWIGPYTHON_BUILTIN) + +SWIGINTERN void +SwigPyBuiltin_AddPublicSymbol(PyObject *seq, const char *key) { + PyObject *s = PyString_InternFromString(key); + PyList_Append(seq, s); + Py_DECREF(s); +} + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, PyObject *public_interface, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); + if (public_interface) + SwigPyBuiltin_AddPublicSymbol(public_interface, name); +} + +#else + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); +} + +#endif + +/* Append a value to the result obj */ + +SWIGINTERN PyObject* +SWIG_Python_AppendOutput(PyObject* result, PyObject* obj) { + if (!result) { + result = obj; + } else if (result == Py_None) { + Py_DECREF(result); + result = obj; + } else { + if (!PyList_Check(result)) { + PyObject *o2 = result; + result = PyList_New(1); + PyList_SetItem(result, 0, o2); + } + PyList_Append(result,obj); + Py_DECREF(obj); + } + return result; +} + +/* Unpack the argument tuple */ + +SWIGINTERN Py_ssize_t +SWIG_Python_UnpackTuple(PyObject *args, const char *name, Py_ssize_t min, Py_ssize_t max, PyObject **objs) +{ + if (!args) { + if (!min && !max) { + return 1; + } else { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got none", + name, (min == max ? "" : "at least "), (int)min); + return 0; + } + } + if (!PyTuple_Check(args)) { + if (min <= 1 && max >= 1) { + Py_ssize_t i; + objs[0] = args; + for (i = 1; i < max; ++i) { + objs[i] = 0; + } + return 2; + } + PyErr_SetString(PyExc_SystemError, "UnpackTuple() argument list is not a tuple"); + return 0; + } else { + Py_ssize_t l = PyTuple_GET_SIZE(args); + if (l < min) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at least "), (int)min, (int)l); + return 0; + } else if (l > max) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at most "), (int)max, (int)l); + return 0; + } else { + Py_ssize_t i; + for (i = 0; i < l; ++i) { + objs[i] = PyTuple_GET_ITEM(args, i); + } + for (; l < max; ++l) { + objs[l] = 0; + } + return i + 1; + } + } +} + +SWIGINTERN int +SWIG_Python_CheckNoKeywords(PyObject *kwargs, const char *name) { + int no_kwargs = 1; + if (kwargs) { + assert(PyDict_Check(kwargs)); + if (PyDict_Size(kwargs) > 0) { + PyErr_Format(PyExc_TypeError, "%s() does not take keyword arguments", name); + no_kwargs = 0; + } + } + return no_kwargs; +} + +/* A functor is a function object with one single object argument */ +#define SWIG_Python_CallFunctor(functor, obj) PyObject_CallFunctionObjArgs(functor, obj, NULL); + +/* + Helper for static pointer initialization for both C and C++ code, for example + static PyObject *SWIG_STATIC_POINTER(MyVar) = NewSomething(...); +*/ +#ifdef __cplusplus +#define SWIG_STATIC_POINTER(var) var +#else +#define SWIG_STATIC_POINTER(var) var = 0; if (!var) var +#endif + +/* ----------------------------------------------------------------------------- + * Pointer declarations + * ----------------------------------------------------------------------------- */ + +/* Flags for new pointer objects */ +#define SWIG_POINTER_NOSHADOW (SWIG_POINTER_OWN << 1) +#define SWIG_POINTER_NEW (SWIG_POINTER_NOSHADOW | SWIG_POINTER_OWN) + +#define SWIG_POINTER_IMPLICIT_CONV (SWIG_POINTER_DISOWN << 1) + +#define SWIG_BUILTIN_TP_INIT (SWIG_POINTER_OWN << 2) +#define SWIG_BUILTIN_INIT (SWIG_BUILTIN_TP_INIT | SWIG_POINTER_OWN) + +#ifdef __cplusplus +extern "C" { +#endif + +/* The python void return value */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Py_Void(void) +{ + PyObject *none = Py_None; + Py_INCREF(none); + return none; +} + +/* SwigPyClientData */ + +typedef struct { + PyObject *klass; + PyObject *newraw; + PyObject *newargs; + PyObject *destroy; + int delargs; + int implicitconv; + PyTypeObject *pytype; +} SwigPyClientData; + +SWIGRUNTIMEINLINE int +SWIG_Python_CheckImplicit(swig_type_info *ty) +{ + SwigPyClientData *data = (SwigPyClientData *)ty->clientdata; + int fail = data ? data->implicitconv : 0; + if (fail) + PyErr_SetString(PyExc_TypeError, "Implicit conversion is prohibited for explicit constructors."); + return fail; +} + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_ExceptionType(swig_type_info *desc) { + SwigPyClientData *data = desc ? (SwigPyClientData *) desc->clientdata : 0; + PyObject *klass = data ? data->klass : 0; + return (klass ? klass : PyExc_RuntimeError); +} + + +SWIGRUNTIME SwigPyClientData * +SwigPyClientData_New(PyObject* obj) +{ + if (!obj) { + return 0; + } else { + SwigPyClientData *data = (SwigPyClientData *)malloc(sizeof(SwigPyClientData)); + /* the klass element */ + data->klass = obj; + Py_INCREF(data->klass); + /* the newraw method and newargs arguments used to create a new raw instance */ + if (PyClass_Check(obj)) { + data->newraw = 0; + data->newargs = obj; + Py_INCREF(obj); + } else { + data->newraw = PyObject_GetAttrString(data->klass, "__new__"); + if (data->newraw) { + Py_INCREF(data->newraw); + data->newargs = PyTuple_New(1); + PyTuple_SetItem(data->newargs, 0, obj); + } else { + data->newargs = obj; + } + Py_INCREF(data->newargs); + } + /* the destroy method, aka as the C++ delete method */ + data->destroy = PyObject_GetAttrString(data->klass, "__swig_destroy__"); + if (PyErr_Occurred()) { + PyErr_Clear(); + data->destroy = 0; + } + if (data->destroy) { + int flags; + Py_INCREF(data->destroy); + flags = PyCFunction_GET_FLAGS(data->destroy); + data->delargs = !(flags & (METH_O)); + } else { + data->delargs = 0; + } + data->implicitconv = 0; + data->pytype = 0; + return data; + } +} + +SWIGRUNTIME void +SwigPyClientData_Del(SwigPyClientData *data) { + Py_XDECREF(data->newraw); + Py_XDECREF(data->newargs); + Py_XDECREF(data->destroy); +} + +/* =============== SwigPyObject =====================*/ + +typedef struct { + PyObject_HEAD + void *ptr; + swig_type_info *ty; + int own; + PyObject *next; +#ifdef SWIGPYTHON_BUILTIN + PyObject *dict; +#endif +} SwigPyObject; + + +#ifdef SWIGPYTHON_BUILTIN + +SWIGRUNTIME PyObject * +SwigPyObject_get___dict__(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + + if (!sobj->dict) + sobj->dict = PyDict_New(); + + Py_INCREF(sobj->dict); + return sobj->dict; +} + +#endif + +SWIGRUNTIME PyObject * +SwigPyObject_long(SwigPyObject *v) +{ + return PyLong_FromVoidPtr(v->ptr); +} + +SWIGRUNTIME PyObject * +SwigPyObject_format(const char* fmt, SwigPyObject *v) +{ + PyObject *res = NULL; + PyObject *args = PyTuple_New(1); + if (args) { + if (PyTuple_SetItem(args, 0, SwigPyObject_long(v)) == 0) { + PyObject *ofmt = SWIG_Python_str_FromChar(fmt); + if (ofmt) { +#if PY_VERSION_HEX >= 0x03000000 + res = PyUnicode_Format(ofmt,args); +#else + res = PyString_Format(ofmt,args); +#endif + Py_DECREF(ofmt); + } + Py_DECREF(args); + } + } + return res; +} + +SWIGRUNTIME PyObject * +SwigPyObject_oct(SwigPyObject *v) +{ + return SwigPyObject_format("%o",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_hex(SwigPyObject *v) +{ + return SwigPyObject_format("%x",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_repr(SwigPyObject *v) +{ + const char *name = SWIG_TypePrettyName(v->ty); + PyObject *repr = SWIG_Python_str_FromFormat("", (name ? name : "unknown"), (void *)v); + if (v->next) { + PyObject *nrep = SwigPyObject_repr((SwigPyObject *)v->next); +# if PY_VERSION_HEX >= 0x03000000 + PyObject *joined = PyUnicode_Concat(repr, nrep); + Py_DecRef(repr); + Py_DecRef(nrep); + repr = joined; +# else + PyString_ConcatAndDel(&repr,nrep); +# endif + } + return repr; +} + +/* We need a version taking two PyObject* parameters so it's a valid + * PyCFunction to use in swigobject_methods[]. */ +SWIGRUNTIME PyObject * +SwigPyObject_repr2(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + return SwigPyObject_repr((SwigPyObject*)v); +} + +SWIGRUNTIME int +SwigPyObject_compare(SwigPyObject *v, SwigPyObject *w) +{ + void *i = v->ptr; + void *j = w->ptr; + return (i < j) ? -1 : ((i > j) ? 1 : 0); +} + +/* Added for Python 3.x, would it also be useful for Python 2.x? */ +SWIGRUNTIME PyObject* +SwigPyObject_richcompare(SwigPyObject *v, SwigPyObject *w, int op) +{ + PyObject* res; + if( op != Py_EQ && op != Py_NE ) { + Py_INCREF(Py_NotImplemented); + return Py_NotImplemented; + } + res = PyBool_FromLong( (SwigPyObject_compare(v, w)==0) == (op == Py_EQ) ? 1 : 0); + return res; +} + + +SWIGRUNTIME PyTypeObject* SwigPyObject_TypeOnce(void); + +#ifdef SWIGPYTHON_BUILTIN +static swig_type_info *SwigPyObject_stype = 0; +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + SwigPyClientData *cd; + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + assert(cd); + assert(cd->pytype); + return cd->pytype; +} +#else +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyObject_TypeOnce(); + return type; +} +#endif + +SWIGRUNTIMEINLINE int +SwigPyObject_Check(PyObject *op) { +#ifdef SWIGPYTHON_BUILTIN + PyTypeObject *target_tp = SwigPyObject_type(); + if (PyType_IsSubtype(op->ob_type, target_tp)) + return 1; + return (strcmp(op->ob_type->tp_name, "SwigPyObject") == 0); +#else + return (Py_TYPE(op) == SwigPyObject_type()) + || (strcmp(Py_TYPE(op)->tp_name,"SwigPyObject") == 0); +#endif +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own); + +SWIGRUNTIME void +SwigPyObject_dealloc(PyObject *v) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + PyObject *next = sobj->next; + if (sobj->own == SWIG_POINTER_OWN) { + swig_type_info *ty = sobj->ty; + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + PyObject *destroy = data ? data->destroy : 0; + if (destroy) { + /* destroy is always a VARARGS method */ + PyObject *res; + + /* PyObject_CallFunction() has the potential to silently drop + the active exception. In cases of unnamed temporary + variable or where we just finished iterating over a generator + StopIteration will be active right now, and this needs to + remain true upon return from SwigPyObject_dealloc. So save + and restore. */ + + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); + + if (data->delargs) { + /* we need to create a temporary object to carry the destroy operation */ + PyObject *tmp = SwigPyObject_New(sobj->ptr, ty, 0); + res = SWIG_Python_CallFunctor(destroy, tmp); + Py_DECREF(tmp); + } else { + PyCFunction meth = PyCFunction_GET_FUNCTION(destroy); + PyObject *mself = PyCFunction_GET_SELF(destroy); + res = ((*meth)(mself, v)); + } + if (!res) + PyErr_WriteUnraisable(destroy); + + PyErr_Restore(type, value, traceback); + + Py_XDECREF(res); + } +#if !defined(SWIG_PYTHON_SILENT_MEMLEAK) + else { + const char *name = SWIG_TypePrettyName(ty); + printf("swig/python detected a memory leak of type '%s', no destructor found.\n", (name ? name : "unknown")); + } +#endif + } + Py_XDECREF(next); + PyObject_DEL(v); +} + +SWIGRUNTIME PyObject* +SwigPyObject_append(PyObject* v, PyObject* next) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (!SwigPyObject_Check(next)) { + PyErr_SetString(PyExc_TypeError, "Attempt to append a non SwigPyObject"); + return NULL; + } + sobj->next = next; + Py_INCREF(next); + return SWIG_Py_Void(); +} + +SWIGRUNTIME PyObject* +SwigPyObject_next(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (sobj->next) { + Py_INCREF(sobj->next); + return sobj->next; + } else { + return SWIG_Py_Void(); + } +} + +SWIGINTERN PyObject* +SwigPyObject_disown(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = 0; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_acquire(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = SWIG_POINTER_OWN; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_own(PyObject *v, PyObject *args) +{ + PyObject *val = 0; + if (!PyArg_UnpackTuple(args, "own", 0, 1, &val)) { + return NULL; + } else { + SwigPyObject *sobj = (SwigPyObject *)v; + PyObject *obj = PyBool_FromLong(sobj->own); + if (val) { + if (PyObject_IsTrue(val)) { + SwigPyObject_acquire(v,args); + } else { + SwigPyObject_disown(v,args); + } + } + return obj; + } +} + +static PyMethodDef +swigobject_methods[] = { + {"disown", SwigPyObject_disown, METH_NOARGS, "releases ownership of the pointer"}, + {"acquire", SwigPyObject_acquire, METH_NOARGS, "acquires ownership of the pointer"}, + {"own", SwigPyObject_own, METH_VARARGS, "returns/sets ownership of the pointer"}, + {"append", SwigPyObject_append, METH_O, "appends another 'this' object"}, + {"next", SwigPyObject_next, METH_NOARGS, "returns the next 'this' object"}, + {"__repr__",SwigPyObject_repr2, METH_NOARGS, "returns object representation"}, + {0, 0, 0, 0} +}; + +SWIGRUNTIME PyTypeObject* +SwigPyObject_TypeOnce(void) { + static char swigobject_doc[] = "Swig object carries a C/C++ instance pointer"; + + static PyNumberMethods SwigPyObject_as_number = { + (binaryfunc)0, /*nb_add*/ + (binaryfunc)0, /*nb_subtract*/ + (binaryfunc)0, /*nb_multiply*/ + /* nb_divide removed in Python 3 */ +#if PY_VERSION_HEX < 0x03000000 + (binaryfunc)0, /*nb_divide*/ +#endif + (binaryfunc)0, /*nb_remainder*/ + (binaryfunc)0, /*nb_divmod*/ + (ternaryfunc)0,/*nb_power*/ + (unaryfunc)0, /*nb_negative*/ + (unaryfunc)0, /*nb_positive*/ + (unaryfunc)0, /*nb_absolute*/ + (inquiry)0, /*nb_nonzero*/ + 0, /*nb_invert*/ + 0, /*nb_lshift*/ + 0, /*nb_rshift*/ + 0, /*nb_and*/ + 0, /*nb_xor*/ + 0, /*nb_or*/ +#if PY_VERSION_HEX < 0x03000000 + 0, /*nb_coerce*/ +#endif + (unaryfunc)SwigPyObject_long, /*nb_int*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_long, /*nb_long*/ +#else + 0, /*nb_reserved*/ +#endif + (unaryfunc)0, /*nb_float*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_oct, /*nb_oct*/ + (unaryfunc)SwigPyObject_hex, /*nb_hex*/ +#endif +#if PY_VERSION_HEX >= 0x03050000 /* 3.5 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_inplace_matrix_multiply */ +#elif PY_VERSION_HEX >= 0x03000000 /* 3.0 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index, nb_inplace_divide removed */ +#else + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index */ +#endif + }; + + static PyTypeObject swigpyobject_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyObject", /* tp_name */ + sizeof(SwigPyObject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyObject_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX >= 0x03000000 + 0, /* tp_reserved in 3.0.1, tp_compare in 3.0.0 but not used */ +#else + (cmpfunc)SwigPyObject_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyObject_repr, /* tp_repr */ + &SwigPyObject_as_number, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + 0, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigobject_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + (richcmpfunc)SwigPyObject_richcompare,/* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + swigobject_methods, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpyobject_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpyobject_type) < 0) + return NULL; + } + return &swigpyobject_type; +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own) +{ + SwigPyObject *sobj = PyObject_NEW(SwigPyObject, SwigPyObject_type()); + if (sobj) { + sobj->ptr = ptr; + sobj->ty = ty; + sobj->own = own; + sobj->next = 0; + } + return (PyObject *)sobj; +} + +/* ----------------------------------------------------------------------------- + * Implements a simple Swig Packed type, and use it instead of string + * ----------------------------------------------------------------------------- */ + +typedef struct { + PyObject_HEAD + void *pack; + swig_type_info *ty; + size_t size; +} SwigPyPacked; + +SWIGRUNTIME PyObject * +SwigPyPacked_repr(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))) { + return SWIG_Python_str_FromFormat("", result, v->ty->name); + } else { + return SWIG_Python_str_FromFormat("", v->ty->name); + } +} + +SWIGRUNTIME PyObject * +SwigPyPacked_str(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))){ + return SWIG_Python_str_FromFormat("%s%s", result, v->ty->name); + } else { + return SWIG_Python_str_FromChar(v->ty->name); + } +} + +SWIGRUNTIME int +SwigPyPacked_compare(SwigPyPacked *v, SwigPyPacked *w) +{ + size_t i = v->size; + size_t j = w->size; + int s = (i < j) ? -1 : ((i > j) ? 1 : 0); + return s ? s : strncmp((const char *)v->pack, (const char *)w->pack, 2*v->size); +} + +SWIGRUNTIME PyTypeObject* SwigPyPacked_TypeOnce(void); + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyPacked_TypeOnce(); + return type; +} + +SWIGRUNTIMEINLINE int +SwigPyPacked_Check(PyObject *op) { + return ((op)->ob_type == SwigPyPacked_TypeOnce()) + || (strcmp((op)->ob_type->tp_name,"SwigPyPacked") == 0); +} + +SWIGRUNTIME void +SwigPyPacked_dealloc(PyObject *v) +{ + if (SwigPyPacked_Check(v)) { + SwigPyPacked *sobj = (SwigPyPacked *) v; + free(sobj->pack); + } + PyObject_DEL(v); +} + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_TypeOnce(void) { + static char swigpacked_doc[] = "Swig object carries a C/C++ instance pointer"; + static PyTypeObject swigpypacked_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX>=0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyPacked", /* tp_name */ + sizeof(SwigPyPacked), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyPacked_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX>=0x03000000 + 0, /* tp_reserved in 3.0.1 */ +#else + (cmpfunc)SwigPyPacked_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyPacked_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + (reprfunc)SwigPyPacked_str, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigpacked_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + 0, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpypacked_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpypacked_type) < 0) + return NULL; + } + return &swigpypacked_type; +} + +SWIGRUNTIME PyObject * +SwigPyPacked_New(void *ptr, size_t size, swig_type_info *ty) +{ + SwigPyPacked *sobj = PyObject_NEW(SwigPyPacked, SwigPyPacked_type()); + if (sobj) { + void *pack = malloc(size); + if (pack) { + memcpy(pack, ptr, size); + sobj->pack = pack; + sobj->ty = ty; + sobj->size = size; + } else { + PyObject_DEL((PyObject *) sobj); + sobj = 0; + } + } + return (PyObject *) sobj; +} + +SWIGRUNTIME swig_type_info * +SwigPyPacked_UnpackData(PyObject *obj, void *ptr, size_t size) +{ + if (SwigPyPacked_Check(obj)) { + SwigPyPacked *sobj = (SwigPyPacked *)obj; + if (sobj->size != size) return 0; + memcpy(ptr, sobj->pack, size); + return sobj->ty; + } else { + return 0; + } +} + +/* ----------------------------------------------------------------------------- + * pointers/data manipulation + * ----------------------------------------------------------------------------- */ + +static PyObject *Swig_This_global = NULL; + +SWIGRUNTIME PyObject * +SWIG_This(void) +{ + if (Swig_This_global == NULL) + Swig_This_global = SWIG_Python_str_FromChar("this"); + return Swig_This_global; +} + +/* #define SWIG_PYTHON_SLOW_GETSET_THIS */ + +/* TODO: I don't know how to implement the fast getset in Python 3 right now */ +#if PY_VERSION_HEX>=0x03000000 +#define SWIG_PYTHON_SLOW_GETSET_THIS +#endif + +SWIGRUNTIME SwigPyObject * +SWIG_Python_GetSwigThis(PyObject *pyobj) +{ + PyObject *obj; + + if (SwigPyObject_Check(pyobj)) + return (SwigPyObject *) pyobj; + +#ifdef SWIGPYTHON_BUILTIN + (void)obj; +# ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + pyobj = PyWeakref_GET_OBJECT(pyobj); + if (pyobj && SwigPyObject_Check(pyobj)) + return (SwigPyObject*) pyobj; + } +# endif + return NULL; +#else + + obj = 0; + +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + if (PyInstance_Check(pyobj)) { + obj = _PyInstance_Lookup(pyobj, SWIG_This()); + } else { + PyObject **dictptr = _PyObject_GetDictPtr(pyobj); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + obj = dict ? PyDict_GetItem(dict, SWIG_This()) : 0; + } else { +#ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + PyObject *wobj = PyWeakref_GET_OBJECT(pyobj); + return wobj ? SWIG_Python_GetSwigThis(wobj) : 0; + } +#endif + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } + } + } +#else + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } +#endif + if (obj && !SwigPyObject_Check(obj)) { + /* a PyObject is called 'this', try to get the 'real this' + SwigPyObject from it */ + return SWIG_Python_GetSwigThis(obj); + } + return (SwigPyObject *)obj; +#endif +} + +/* Acquire a pointer value */ + +SWIGRUNTIME int +SWIG_Python_AcquirePtr(PyObject *obj, int own) { + if (own == SWIG_POINTER_OWN) { + SwigPyObject *sobj = SWIG_Python_GetSwigThis(obj); + if (sobj) { + int oldown = sobj->own; + sobj->own = own; + return oldown; + } + } + return 0; +} + +/* Convert a pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPtrAndOwn(PyObject *obj, void **ptr, swig_type_info *ty, int flags, int *own) { + int res; + SwigPyObject *sobj; + int implicit_conv = (flags & SWIG_POINTER_IMPLICIT_CONV) != 0; + + if (!obj) + return SWIG_ERROR; + if (obj == Py_None && !implicit_conv) { + if (ptr) + *ptr = 0; + return (flags & SWIG_POINTER_NO_NULL) ? SWIG_NullReferenceError : SWIG_OK; + } + + res = SWIG_ERROR; + + sobj = SWIG_Python_GetSwigThis(obj); + if (own) + *own = 0; + while (sobj) { + void *vptr = sobj->ptr; + if (ty) { + swig_type_info *to = sobj->ty; + if (to == ty) { + /* no type cast needed */ + if (ptr) *ptr = vptr; + break; + } else { + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) { + sobj = (SwigPyObject *)sobj->next; + } else { + if (ptr) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + if (newmemory == SWIG_CAST_NEW_MEMORY) { + assert(own); /* badly formed typemap which will lead to a memory leak - it must set and use own to delete *ptr */ + if (own) + *own = *own | SWIG_CAST_NEW_MEMORY; + } + } + break; + } + } + } else { + if (ptr) *ptr = vptr; + break; + } + } + if (sobj) { + if (own) + *own = *own | sobj->own; + if (flags & SWIG_POINTER_DISOWN) { + sobj->own = 0; + } + res = SWIG_OK; + } else { + if (implicit_conv) { + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + if (data && !data->implicitconv) { + PyObject *klass = data->klass; + if (klass) { + PyObject *impconv; + data->implicitconv = 1; /* avoid recursion and call 'explicit' constructors*/ + impconv = SWIG_Python_CallFunctor(klass, obj); + data->implicitconv = 0; + if (PyErr_Occurred()) { + PyErr_Clear(); + impconv = 0; + } + if (impconv) { + SwigPyObject *iobj = SWIG_Python_GetSwigThis(impconv); + if (iobj) { + void *vptr; + res = SWIG_Python_ConvertPtrAndOwn((PyObject*)iobj, &vptr, ty, 0, 0); + if (SWIG_IsOK(res)) { + if (ptr) { + *ptr = vptr; + /* transfer the ownership to 'ptr' */ + iobj->own = 0; + res = SWIG_AddCast(res); + res = SWIG_AddNewMask(res); + } else { + res = SWIG_AddCast(res); + } + } + } + Py_DECREF(impconv); + } + } + } + if (!SWIG_IsOK(res) && obj == Py_None) { + if (ptr) + *ptr = 0; + if (PyErr_Occurred()) + PyErr_Clear(); + res = SWIG_OK; + } + } + } + return res; +} + +/* Convert a function ptr value */ + +SWIGRUNTIME int +SWIG_Python_ConvertFunctionPtr(PyObject *obj, void **ptr, swig_type_info *ty) { + if (!PyCFunction_Check(obj)) { + return SWIG_ConvertPtr(obj, ptr, ty, 0); + } else { + void *vptr = 0; + swig_cast_info *tc; + + /* here we get the method pointer for callbacks */ + const char *doc = (((PyCFunctionObject *)obj) -> m_ml -> ml_doc); + const char *desc = doc ? strstr(doc, "swig_ptr: ") : 0; + if (desc) + desc = ty ? SWIG_UnpackVoidPtr(desc + 10, &vptr, ty->name) : 0; + if (!desc) + return SWIG_ERROR; + tc = SWIG_TypeCheck(desc,ty); + if (tc) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + assert(!newmemory); /* newmemory handling not yet implemented */ + } else { + return SWIG_ERROR; + } + return SWIG_OK; + } +} + +/* Convert a packed pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPacked(PyObject *obj, void *ptr, size_t sz, swig_type_info *ty) { + swig_type_info *to = SwigPyPacked_UnpackData(obj, ptr, sz); + if (!to) return SWIG_ERROR; + if (ty) { + if (to != ty) { + /* check type cast? */ + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) return SWIG_ERROR; + } + } + return SWIG_OK; +} + +/* ----------------------------------------------------------------------------- + * Create a new pointer object + * ----------------------------------------------------------------------------- */ + +/* + Create a new instance object, without calling __init__, and set the + 'this' attribute. +*/ + +SWIGRUNTIME PyObject* +SWIG_Python_NewShadowInstance(SwigPyClientData *data, PyObject *swig_this) +{ + PyObject *inst = 0; + PyObject *newraw = data->newraw; + if (newraw) { + inst = PyObject_Call(newraw, data->newargs, NULL); + if (inst) { +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + PyDict_SetItem(dict, SWIG_This(), swig_this); + } + } +#else + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } +#endif + } + } else { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *empty_args = PyTuple_New(0); + if (empty_args) { + PyObject *empty_kwargs = PyDict_New(); + if (empty_kwargs) { + inst = ((PyTypeObject *)data->newargs)->tp_new((PyTypeObject *)data->newargs, empty_args, empty_kwargs); + Py_DECREF(empty_kwargs); + if (inst) { + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } else { + Py_TYPE(inst)->tp_flags &= ~Py_TPFLAGS_VALID_VERSION_TAG; + } + } + } + Py_DECREF(empty_args); + } +#else + PyObject *dict = PyDict_New(); + if (dict) { + PyDict_SetItem(dict, SWIG_This(), swig_this); + inst = PyInstance_NewRaw(data->newargs, dict); + Py_DECREF(dict); + } +#endif + } + return inst; +} + +SWIGRUNTIME int +SWIG_Python_SetSwigThis(PyObject *inst, PyObject *swig_this) +{ +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + } + return PyDict_SetItem(dict, SWIG_This(), swig_this); + } +#endif + return PyObject_SetAttr(inst, SWIG_This(), swig_this); +} + + +SWIGINTERN PyObject * +SWIG_Python_InitShadowInstance(PyObject *args) { + PyObject *obj[2]; + if (!SWIG_Python_UnpackTuple(args, "swiginit", 2, 2, obj)) { + return NULL; + } else { + SwigPyObject *sthis = SWIG_Python_GetSwigThis(obj[0]); + if (sthis) { + SwigPyObject_append((PyObject*) sthis, obj[1]); + } else { + if (SWIG_Python_SetSwigThis(obj[0], obj[1]) != 0) + return NULL; + } + return SWIG_Py_Void(); + } +} + +/* Create a new pointer object */ + +SWIGRUNTIME PyObject * +SWIG_Python_NewPointerObj(PyObject *self, void *ptr, swig_type_info *type, int flags) { + SwigPyClientData *clientdata; + PyObject * robj; + int own; + + if (!ptr) + return SWIG_Py_Void(); + + clientdata = type ? (SwigPyClientData *)(type->clientdata) : 0; + own = (flags & SWIG_POINTER_OWN) ? SWIG_POINTER_OWN : 0; + if (clientdata && clientdata->pytype) { + SwigPyObject *newobj; + if (flags & SWIG_BUILTIN_TP_INIT) { + newobj = (SwigPyObject*) self; + if (newobj->ptr) { + PyObject *next_self = clientdata->pytype->tp_alloc(clientdata->pytype, 0); + while (newobj->next) + newobj = (SwigPyObject *) newobj->next; + newobj->next = next_self; + newobj = (SwigPyObject *)next_self; +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + } else { + newobj = PyObject_New(SwigPyObject, clientdata->pytype); +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + if (newobj) { + newobj->ptr = ptr; + newobj->ty = type; + newobj->own = own; + newobj->next = 0; + return (PyObject*) newobj; + } + return SWIG_Py_Void(); + } + + assert(!(flags & SWIG_BUILTIN_TP_INIT)); + + robj = SwigPyObject_New(ptr, type, own); + if (robj && clientdata && !(flags & SWIG_POINTER_NOSHADOW)) { + PyObject *inst = SWIG_Python_NewShadowInstance(clientdata, robj); + Py_DECREF(robj); + robj = inst; + } + return robj; +} + +/* Create a new packed object */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_NewPackedObj(void *ptr, size_t sz, swig_type_info *type) { + return ptr ? SwigPyPacked_New((void *) ptr, sz, type) : SWIG_Py_Void(); +} + +/* -----------------------------------------------------------------------------* + * Get type list + * -----------------------------------------------------------------------------*/ + +#ifdef SWIG_LINK_RUNTIME +void *SWIG_ReturnGlobalTypeList(void *); +#endif + +SWIGRUNTIME swig_module_info * +SWIG_Python_GetModule(void *SWIGUNUSEDPARM(clientdata)) { + static void *type_pointer = (void *)0; + /* first check if module already created */ + if (!type_pointer) { +#ifdef SWIG_LINK_RUNTIME + type_pointer = SWIG_ReturnGlobalTypeList((void *)0); +#else + type_pointer = PyCapsule_Import(SWIGPY_CAPSULE_NAME, 0); + if (PyErr_Occurred()) { + PyErr_Clear(); + type_pointer = (void *)0; + } +#endif + } + return (swig_module_info *) type_pointer; +} + +SWIGRUNTIME void +SWIG_Python_DestroyModule(PyObject *obj) +{ + swig_module_info *swig_module = (swig_module_info *) PyCapsule_GetPointer(obj, SWIGPY_CAPSULE_NAME); + swig_type_info **types = swig_module->types; + size_t i; + for (i =0; i < swig_module->size; ++i) { + swig_type_info *ty = types[i]; + if (ty->owndata) { + SwigPyClientData *data = (SwigPyClientData *) ty->clientdata; + if (data) SwigPyClientData_Del(data); + } + } + Py_DECREF(SWIG_This()); + Swig_This_global = NULL; +} + +SWIGRUNTIME void +SWIG_Python_SetModule(swig_module_info *swig_module) { +#if PY_VERSION_HEX >= 0x03000000 + /* Add a dummy module object into sys.modules */ + PyObject *module = PyImport_AddModule("swig_runtime_data" SWIG_RUNTIME_VERSION); +#else + static PyMethodDef swig_empty_runtime_method_table[] = { {NULL, NULL, 0, NULL} }; /* Sentinel */ + PyObject *module = Py_InitModule("swig_runtime_data" SWIG_RUNTIME_VERSION, swig_empty_runtime_method_table); +#endif + PyObject *pointer = PyCapsule_New((void *) swig_module, SWIGPY_CAPSULE_NAME, SWIG_Python_DestroyModule); + if (pointer && module) { + PyModule_AddObject(module, "type_pointer_capsule" SWIG_TYPE_TABLE_NAME, pointer); + } else { + Py_XDECREF(pointer); + } +} + +/* The python cached type query */ +SWIGRUNTIME PyObject * +SWIG_Python_TypeCache(void) { + static PyObject *SWIG_STATIC_POINTER(cache) = PyDict_New(); + return cache; +} + +SWIGRUNTIME swig_type_info * +SWIG_Python_TypeQuery(const char *type) +{ + PyObject *cache = SWIG_Python_TypeCache(); + PyObject *key = SWIG_Python_str_FromChar(type); + PyObject *obj = PyDict_GetItem(cache, key); + swig_type_info *descriptor; + if (obj) { + descriptor = (swig_type_info *) PyCapsule_GetPointer(obj, NULL); + } else { + swig_module_info *swig_module = SWIG_GetModule(0); + descriptor = SWIG_TypeQueryModule(swig_module, swig_module, type); + if (descriptor) { + obj = PyCapsule_New((void*) descriptor, NULL, NULL); + PyDict_SetItem(cache, key, obj); + Py_DECREF(obj); + } + } + Py_DECREF(key); + return descriptor; +} + +/* + For backward compatibility only +*/ +#define SWIG_POINTER_EXCEPTION 0 +#define SWIG_arg_fail(arg) SWIG_Python_ArgFail(arg) +#define SWIG_MustGetPtr(p, type, argnum, flags) SWIG_Python_MustGetPtr(p, type, argnum, flags) + +SWIGRUNTIME int +SWIG_Python_AddErrMesg(const char* mesg, int infront) +{ + if (PyErr_Occurred()) { + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + const char *errmesg = tmp ? tmp : "Invalid error message"; + Py_XINCREF(type); + PyErr_Clear(); + if (infront) { + PyErr_Format(type, "%s %s", mesg, errmesg); + } else { + PyErr_Format(type, "%s %s", errmesg, mesg); + } + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + } + return 1; + } else { + return 0; + } +} + +SWIGRUNTIME int +SWIG_Python_ArgFail(int argnum) +{ + if (PyErr_Occurred()) { + /* add information about failing argument */ + char mesg[256]; + PyOS_snprintf(mesg, sizeof(mesg), "argument number %d:", argnum); + return SWIG_Python_AddErrMesg(mesg, 1); + } else { + return 0; + } +} + +SWIGRUNTIMEINLINE const char * +SwigPyObject_GetDesc(PyObject *self) +{ + SwigPyObject *v = (SwigPyObject *)self; + swig_type_info *ty = v ? v->ty : 0; + return ty ? ty->str : ""; +} + +SWIGRUNTIME void +SWIG_Python_TypeError(const char *type, PyObject *obj) +{ + if (type) { +#if defined(SWIG_COBJECT_TYPES) + if (obj && SwigPyObject_Check(obj)) { + const char *otype = (const char *) SwigPyObject_GetDesc(obj); + if (otype) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, 'SwigPyObject(%s)' is received", + type, otype); + return; + } + } else +#endif + { + const char *otype = (obj ? obj->ob_type->tp_name : 0); + if (otype) { + PyObject *str = PyObject_Str(obj); + const char *cstr = str ? SWIG_Python_str_AsChar(str) : 0; + if (cstr) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s(%s)' is received", + type, otype, cstr); + SWIG_Python_str_DelForPy3(cstr); + } else { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s' is received", + type, otype); + } + Py_XDECREF(str); + return; + } + } + PyErr_Format(PyExc_TypeError, "a '%s' is expected", type); + } else { + PyErr_Format(PyExc_TypeError, "unexpected type is received"); + } +} + + +/* Convert a pointer value, signal an exception on a type mismatch */ +SWIGRUNTIME void * +SWIG_Python_MustGetPtr(PyObject *obj, swig_type_info *ty, int SWIGUNUSEDPARM(argnum), int flags) { + void *result; + if (SWIG_Python_ConvertPtr(obj, &result, ty, flags) == -1) { + PyErr_Clear(); +#if SWIG_POINTER_EXCEPTION + if (flags) { + SWIG_Python_TypeError(SWIG_TypePrettyName(ty), obj); + SWIG_Python_ArgFail(argnum); + } +#endif + } + return result; +} + +#ifdef SWIGPYTHON_BUILTIN +SWIGRUNTIME int +SWIG_Python_NonDynamicSetAttr(PyObject *obj, PyObject *name, PyObject *value) { + PyTypeObject *tp = obj->ob_type; + PyObject *descr; + PyObject *encoded_name; + descrsetfunc f; + int res = -1; + +# ifdef Py_USING_UNICODE + if (PyString_Check(name)) { + name = PyUnicode_Decode(PyString_AsString(name), PyString_Size(name), NULL, NULL); + if (!name) + return -1; + } else if (!PyUnicode_Check(name)) +# else + if (!PyString_Check(name)) +# endif + { + PyErr_Format(PyExc_TypeError, "attribute name must be string, not '%.200s'", name->ob_type->tp_name); + return -1; + } else { + Py_INCREF(name); + } + + if (!tp->tp_dict) { + if (PyType_Ready(tp) < 0) + goto done; + } + + descr = _PyType_Lookup(tp, name); + f = NULL; + if (descr != NULL) + f = descr->ob_type->tp_descr_set; + if (!f) { + if (PyString_Check(name)) { + encoded_name = name; + Py_INCREF(name); + } else { + encoded_name = PyUnicode_AsUTF8String(name); + if (!encoded_name) + return -1; + } + PyErr_Format(PyExc_AttributeError, "'%.100s' object has no attribute '%.200s'", tp->tp_name, PyString_AsString(encoded_name)); + Py_DECREF(encoded_name); + } else { + res = f(descr, obj, value); + } + + done: + Py_DECREF(name); + return res; +} +#endif + + +#ifdef __cplusplus +} +#endif + + + +#define SWIG_exception_fail(code, msg) do { SWIG_Error(code, msg); SWIG_fail; } while(0) + +#define SWIG_contract_assert(expr, msg) if (!(expr)) { SWIG_Error(SWIG_RuntimeError, msg); SWIG_fail; } else + + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Method creation and docstring support functions */ + +SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name); +SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); +SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); + +#ifdef __cplusplus +} +#endif + + +/* -------- TYPES TABLE (BEGIN) -------- */ + +#define SWIGTYPE_p_ControlMode swig_types[0] +#define SWIGTYPE_p_CoordinateSystem swig_types[1] +#define SWIGTYPE_p_EngineType swig_types[2] +#define SWIGTYPE_p_IndicatorState swig_types[3] +#define SWIGTYPE_p_Mode swig_types[4] +#define SWIGTYPE_p_SimulationMode swig_types[5] +#define SWIGTYPE_p_Type swig_types[6] +#define SWIGTYPE_p_UserInputEvent swig_types[7] +#define SWIGTYPE_p_WbCameraRecognitionObject swig_types[8] +#define SWIGTYPE_p_WbContactPoint swig_types[9] +#define SWIGTYPE_p_WbLidarPoint swig_types[10] +#define SWIGTYPE_p_WbMouseState swig_types[11] +#define SWIGTYPE_p_WbRadarTarget swig_types[12] +#define SWIGTYPE_p_WheelIndex swig_types[13] +#define SWIGTYPE_p_WiperMode swig_types[14] +#define SWIGTYPE_p_char swig_types[15] +#define SWIGTYPE_p_webots__Car swig_types[16] +#define SWIGTYPE_p_webots__Driver swig_types[17] +#define SWIGTYPE_p_webots__Robot swig_types[18] +#define SWIGTYPE_p_webots__Supervisor swig_types[19] +static swig_type_info *swig_types[21]; +static swig_module_info swig_module = {swig_types, 20, 0, 0, 0, 0}; +#define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name) +#define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name) + +/* -------- TYPES TABLE (END) -------- */ + +#ifdef SWIG_TypeQuery +# undef SWIG_TypeQuery +#endif +#define SWIG_TypeQuery SWIG_Python_TypeQuery + +/*----------------------------------------------- + @(target):= _vehicle.so + ------------------------------------------------*/ +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_init PyInit__vehicle + +#else +# define SWIG_init init_vehicle + +#endif +#define SWIG_name "_vehicle" + +#define SWIGVERSION 0x040002 +#define SWIG_VERSION SWIGVERSION + + +#define SWIG_as_voidptr(a) const_cast< void * >(static_cast< const void * >(a)) +#define SWIG_as_voidptrptr(a) ((void)SWIG_as_voidptr(*a),reinterpret_cast< void** >(a)) + + +#include + + +namespace swig { + class SwigPtr_PyObject { + protected: + PyObject *_obj; + + public: + SwigPtr_PyObject() :_obj(0) + { + } + + SwigPtr_PyObject(const SwigPtr_PyObject& item) : _obj(item._obj) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + SwigPtr_PyObject(PyObject *obj, bool initial_ref = true) :_obj(obj) + { + if (initial_ref) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + } + + SwigPtr_PyObject & operator=(const SwigPtr_PyObject& item) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(item._obj); + Py_XDECREF(_obj); + _obj = item._obj; + SWIG_PYTHON_THREAD_END_BLOCK; + return *this; + } + + ~SwigPtr_PyObject() + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XDECREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + operator PyObject *() const + { + return _obj; + } + + PyObject *operator->() const + { + return _obj; + } + }; +} + + +namespace swig { + struct SwigVar_PyObject : SwigPtr_PyObject { + SwigVar_PyObject(PyObject* obj = 0) : SwigPtr_PyObject(obj, false) { } + + SwigVar_PyObject & operator = (PyObject* obj) + { + Py_XDECREF(_obj); + _obj = obj; + return *this; + } + }; +} + + +#include +#include + + +SWIGINTERNINLINE PyObject* + SWIG_From_int (int value) +{ + return PyInt_FromLong((long) value); +} + + +SWIGINTERNINLINE PyObject* + SWIG_From_bool (bool value) +{ + return PyBool_FromLong(value ? 1 : 0); +} + + +SWIGINTERN int +SWIG_AsVal_double (PyObject *obj, double *val) +{ + int res = SWIG_TypeError; + if (PyFloat_Check(obj)) { + if (val) *val = PyFloat_AsDouble(obj); + return SWIG_OK; +#if PY_VERSION_HEX < 0x03000000 + } else if (PyInt_Check(obj)) { + if (val) *val = (double) PyInt_AsLong(obj); + return SWIG_OK; +#endif + } else if (PyLong_Check(obj)) { + double v = PyLong_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + double d = PyFloat_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = d; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_AddCast(SWIG_OK)); + } else { + PyErr_Clear(); + } + } + } +#endif + return res; +} + + + #define SWIG_From_double PyFloat_FromDouble + + +#include +#if !defined(SWIG_NO_LLONG_MAX) +# if !defined(LLONG_MAX) && defined(__GNUC__) && defined (__LONG_LONG_MAX__) +# define LLONG_MAX __LONG_LONG_MAX__ +# define LLONG_MIN (-LLONG_MAX - 1LL) +# define ULLONG_MAX (LLONG_MAX * 2ULL + 1ULL) +# endif +#endif + + +#include + + +#include + + +SWIGINTERNINLINE int +SWIG_CanCastAsInteger(double *d, double min, double max) { + double x = *d; + if ((min <= x && x <= max)) { + double fx = floor(x); + double cx = ceil(x); + double rd = ((x - fx) < 0.5) ? fx : cx; /* simple rint */ + if ((errno == EDOM) || (errno == ERANGE)) { + errno = 0; + } else { + double summ, reps, diff; + if (rd < x) { + diff = x - rd; + } else if (rd > x) { + diff = rd - x; + } else { + return 1; + } + summ = rd + x; + reps = diff/summ; + if (reps < 8*DBL_EPSILON) { + *d = rd; + return 1; + } + } + } + return 0; +} + + +SWIGINTERN int +SWIG_AsVal_long (PyObject *obj, long* val) +{ +#if PY_VERSION_HEX < 0x03000000 + if (PyInt_Check(obj)) { + if (val) *val = PyInt_AsLong(obj); + return SWIG_OK; + } else +#endif + if (PyLong_Check(obj)) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + return SWIG_OverflowError; + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + long v = PyInt_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + double d; + int res = SWIG_AddCast(SWIG_AsVal_double (obj,&d)); + if (SWIG_IsOK(res) && SWIG_CanCastAsInteger(&d, LONG_MIN, LONG_MAX)) { + if (val) *val = (long)(d); + return res; + } + } + } +#endif + return SWIG_TypeError; +} + + +SWIGINTERN int +SWIG_AsVal_int (PyObject * obj, int *val) +{ + long v; + int res = SWIG_AsVal_long (obj, &v); + if (SWIG_IsOK(res)) { + if ((v < INT_MIN || v > INT_MAX)) { + return SWIG_OverflowError; + } else { + if (val) *val = static_cast< int >(v); + } + } + return res; +} + + +SWIGINTERN int +SWIG_AsVal_bool (PyObject *obj, bool *val) +{ + int r; + if (!PyBool_Check(obj)) + return SWIG_ERROR; + r = PyObject_IsTrue(obj); + if (r == -1) + return SWIG_ERROR; + if (val) *val = r ? true : false; + return SWIG_OK; +} + +#ifdef __cplusplus +extern "C" { +#endif +SWIGINTERN PyObject *_wrap_new_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Driver", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)new webots::Driver(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDriverInstance(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_getDriverInstance", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)webots::Driver::getDriverInstance(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, 0 | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Driver" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_isInitialisationPossible(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + bool result; + + if (!SWIG_Python_UnpackTuple(args, "Driver_isInitialisationPossible", 0, 0, 0)) SWIG_fail; + result = (bool)webots::Driver::isInitialisationPossible(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_step(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_step" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->step(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setCruisingSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setCruisingSpeed" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setCruisingSpeed(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getTargetCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getTargetCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getTargetCruisingSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getCurrentSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getCurrentSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getCurrentSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setThrottle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setThrottle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setThrottle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getThrottle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrakeIntensity", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrakeIntensity" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrakeIntensity(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getBrakeIntensity(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::IndicatorState arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setIndicator", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setIndicator" "', argument " "2"" of type '" "webots::Driver::IndicatorState""'"); + } + arg2 = static_cast< webots::Driver::IndicatorState >(val2); + (arg1)->setIndicator(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setHazardFlashers", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setHazardFlashers" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setHazardFlashers(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::IndicatorState result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::IndicatorState)(arg1)->getIndicator(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getHazardFlashers(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setDippedBeams", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setDippedBeams" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setDippedBeams(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setAntifogLights", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setAntifogLights" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setAntifogLights(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getDippedBeams(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getAntifogLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getRpm(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getRpm" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getRpm(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGear(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + int arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setGear", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setGear" "', argument " "2"" of type '" "int""'"); + } + arg2 = static_cast< int >(val2); + (arg1)->setGear(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGearNumber(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGearNumber" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGearNumber(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getControlMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::ControlMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getControlMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::ControlMode)(arg1)->getControlMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWiperMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWiperMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWiperMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWiperMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrake(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrake", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrake" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrake" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrake(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWipersMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWipersMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWipersMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWipersMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Driver_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Driver, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Driver_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +SWIGINTERN PyObject *_wrap_new_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Car", 0, 0, 0)) SWIG_fail; + result = (webots::Car *)new webots::Car(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Car, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Car" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::Type result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::Type)(arg1)->getType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getEngineType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::EngineType result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getEngineType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::EngineType)(arg1)->getEngineType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setIndicatorPeriod", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setIndicatorPeriod" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setIndicatorPeriod(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getIndicatorPeriod(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBackwardsLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBackwardsLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBackwardsLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBrakeLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBrakeLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBrakeLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackFront(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackFront" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackFront(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackRear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackRear" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackRear(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelbase(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelbase" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getWheelbase(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getFrontWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getFrontWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getFrontWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRearWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRearWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRearWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelEncoder(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelEncoder", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelEncoder" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelEncoder" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelEncoder(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelSpeed" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelSpeed" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelSpeed(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setLeftSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setLeftSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setLeftSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setRightSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setRightSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setRightSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRightSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getLeftSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableLimitedSlipDifferential(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableLimitedSlipDifferential", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableLimitedSlipDifferential(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableIndicatorAutoDisabling(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableIndicatorAutoDisabling", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableIndicatorAutoDisabling(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Car_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Car, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Car_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +static PyMethodDef SwigMethods[] = { + { "SWIG_PyInstanceMethod_New", SWIG_PyInstanceMethod_New, METH_O, NULL}, + { "new_Driver", _wrap_new_Driver, METH_NOARGS, NULL}, + { "Driver_getDriverInstance", _wrap_Driver_getDriverInstance, METH_NOARGS, NULL}, + { "delete_Driver", _wrap_delete_Driver, METH_O, NULL}, + { "Driver_isInitialisationPossible", _wrap_Driver_isInitialisationPossible, METH_NOARGS, NULL}, + { "Driver_step", _wrap_Driver_step, METH_O, NULL}, + { "Driver_setSteeringAngle", _wrap_Driver_setSteeringAngle, METH_VARARGS, NULL}, + { "Driver_getSteeringAngle", _wrap_Driver_getSteeringAngle, METH_O, NULL}, + { "Driver_setCruisingSpeed", _wrap_Driver_setCruisingSpeed, METH_VARARGS, NULL}, + { "Driver_getTargetCruisingSpeed", _wrap_Driver_getTargetCruisingSpeed, METH_O, NULL}, + { "Driver_getCurrentSpeed", _wrap_Driver_getCurrentSpeed, METH_O, NULL}, + { "Driver_setThrottle", _wrap_Driver_setThrottle, METH_VARARGS, NULL}, + { "Driver_getThrottle", _wrap_Driver_getThrottle, METH_O, NULL}, + { "Driver_setBrakeIntensity", _wrap_Driver_setBrakeIntensity, METH_VARARGS, NULL}, + { "Driver_getBrakeIntensity", _wrap_Driver_getBrakeIntensity, METH_O, NULL}, + { "Driver_setIndicator", _wrap_Driver_setIndicator, METH_VARARGS, NULL}, + { "Driver_setHazardFlashers", _wrap_Driver_setHazardFlashers, METH_VARARGS, NULL}, + { "Driver_getIndicator", _wrap_Driver_getIndicator, METH_O, NULL}, + { "Driver_getHazardFlashers", _wrap_Driver_getHazardFlashers, METH_O, NULL}, + { "Driver_setDippedBeams", _wrap_Driver_setDippedBeams, METH_VARARGS, NULL}, + { "Driver_setAntifogLights", _wrap_Driver_setAntifogLights, METH_VARARGS, NULL}, + { "Driver_getDippedBeams", _wrap_Driver_getDippedBeams, METH_O, NULL}, + { "Driver_getAntifogLights", _wrap_Driver_getAntifogLights, METH_O, NULL}, + { "Driver_getRpm", _wrap_Driver_getRpm, METH_O, NULL}, + { "Driver_getGear", _wrap_Driver_getGear, METH_O, NULL}, + { "Driver_setGear", _wrap_Driver_setGear, METH_VARARGS, NULL}, + { "Driver_getGearNumber", _wrap_Driver_getGearNumber, METH_O, NULL}, + { "Driver_getControlMode", _wrap_Driver_getControlMode, METH_O, NULL}, + { "Driver_setWiperMode", _wrap_Driver_setWiperMode, METH_VARARGS, NULL}, + { "Driver_getWiperMode", _wrap_Driver_getWiperMode, METH_O, NULL}, + { "Driver_setBrake", _wrap_Driver_setBrake, METH_VARARGS, NULL}, + { "Driver_setWipersMode", _wrap_Driver_setWipersMode, METH_VARARGS, NULL}, + { "Driver_getWipersMode", _wrap_Driver_getWipersMode, METH_O, NULL}, + { "Driver_swigregister", Driver_swigregister, METH_O, NULL}, + { "Driver_swiginit", Driver_swiginit, METH_VARARGS, NULL}, + { "new_Car", _wrap_new_Car, METH_NOARGS, NULL}, + { "delete_Car", _wrap_delete_Car, METH_O, NULL}, + { "Car_getType", _wrap_Car_getType, METH_O, NULL}, + { "Car_getEngineType", _wrap_Car_getEngineType, METH_O, NULL}, + { "Car_setIndicatorPeriod", _wrap_Car_setIndicatorPeriod, METH_VARARGS, NULL}, + { "Car_getIndicatorPeriod", _wrap_Car_getIndicatorPeriod, METH_O, NULL}, + { "Car_getBackwardsLights", _wrap_Car_getBackwardsLights, METH_O, NULL}, + { "Car_getBrakeLights", _wrap_Car_getBrakeLights, METH_O, NULL}, + { "Car_getTrackFront", _wrap_Car_getTrackFront, METH_O, NULL}, + { "Car_getTrackRear", _wrap_Car_getTrackRear, METH_O, NULL}, + { "Car_getWheelbase", _wrap_Car_getWheelbase, METH_O, NULL}, + { "Car_getFrontWheelRadius", _wrap_Car_getFrontWheelRadius, METH_O, NULL}, + { "Car_getRearWheelRadius", _wrap_Car_getRearWheelRadius, METH_O, NULL}, + { "Car_getWheelEncoder", _wrap_Car_getWheelEncoder, METH_VARARGS, NULL}, + { "Car_getWheelSpeed", _wrap_Car_getWheelSpeed, METH_VARARGS, NULL}, + { "Car_setLeftSteeringAngle", _wrap_Car_setLeftSteeringAngle, METH_VARARGS, NULL}, + { "Car_setRightSteeringAngle", _wrap_Car_setRightSteeringAngle, METH_VARARGS, NULL}, + { "Car_getRightSteeringAngle", _wrap_Car_getRightSteeringAngle, METH_O, NULL}, + { "Car_getLeftSteeringAngle", _wrap_Car_getLeftSteeringAngle, METH_O, NULL}, + { "Car_enableLimitedSlipDifferential", _wrap_Car_enableLimitedSlipDifferential, METH_VARARGS, NULL}, + { "Car_enableIndicatorAutoDisabling", _wrap_Car_enableIndicatorAutoDisabling, METH_VARARGS, NULL}, + { "Car_swigregister", Car_swigregister, METH_O, NULL}, + { "Car_swiginit", Car_swiginit, METH_VARARGS, NULL}, + { NULL, NULL, 0, NULL } +}; + +static PyMethodDef SwigMethods_proxydocs[] = { + { NULL, NULL, 0, NULL } +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (BEGIN) -------- */ + +static void *_p_webots__DriverTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__CarTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) (webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__DriverTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__SupervisorTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) ((webots::Supervisor *) x)); +} +static void *_p_webots__CarTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *)(webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__CarTo_p_webots__Driver(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Driver *) ((webots::Car *) x)); +} +static swig_type_info _swigt__p_ControlMode = {"_p_ControlMode", "ControlMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_CoordinateSystem = {"_p_CoordinateSystem", "CoordinateSystem *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_EngineType = {"_p_EngineType", "EngineType *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_IndicatorState = {"_p_IndicatorState", "IndicatorState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Mode = {"_p_Mode", "Mode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_SimulationMode = {"_p_SimulationMode", "SimulationMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Type = {"_p_Type", "Type *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_UserInputEvent = {"_p_UserInputEvent", "UserInputEvent *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbCameraRecognitionObject = {"_p_WbCameraRecognitionObject", "WbCameraRecognitionObject *|webots::CameraRecognitionObject *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbContactPoint = {"_p_WbContactPoint", "WbContactPoint *|webots::ContactPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbLidarPoint = {"_p_WbLidarPoint", "WbLidarPoint *|webots::LidarPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbMouseState = {"_p_WbMouseState", "WbMouseState *|webots::MouseState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbRadarTarget = {"_p_WbRadarTarget", "WbRadarTarget *|webots::RadarTarget *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WheelIndex = {"_p_WheelIndex", "WheelIndex *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WiperMode = {"_p_WiperMode", "WiperMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_char = {"_p_char", "char *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Car = {"_p_webots__Car", "webots::Car *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Driver = {"_p_webots__Driver", "webots::Driver *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Robot = {"_p_webots__Robot", "webots::Robot *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Supervisor = {"_p_webots__Supervisor", "webots::Supervisor *", 0, 0, (void*)0, 0}; + +static swig_type_info *swig_type_initial[] = { + &_swigt__p_ControlMode, + &_swigt__p_CoordinateSystem, + &_swigt__p_EngineType, + &_swigt__p_IndicatorState, + &_swigt__p_Mode, + &_swigt__p_SimulationMode, + &_swigt__p_Type, + &_swigt__p_UserInputEvent, + &_swigt__p_WbCameraRecognitionObject, + &_swigt__p_WbContactPoint, + &_swigt__p_WbLidarPoint, + &_swigt__p_WbMouseState, + &_swigt__p_WbRadarTarget, + &_swigt__p_WheelIndex, + &_swigt__p_WiperMode, + &_swigt__p_char, + &_swigt__p_webots__Car, + &_swigt__p_webots__Driver, + &_swigt__p_webots__Robot, + &_swigt__p_webots__Supervisor, +}; + +static swig_cast_info _swigc__p_ControlMode[] = { {&_swigt__p_ControlMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_CoordinateSystem[] = { {&_swigt__p_CoordinateSystem, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_EngineType[] = { {&_swigt__p_EngineType, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_IndicatorState[] = { {&_swigt__p_IndicatorState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Mode[] = { {&_swigt__p_Mode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_SimulationMode[] = { {&_swigt__p_SimulationMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Type[] = { {&_swigt__p_Type, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_UserInputEvent[] = { {&_swigt__p_UserInputEvent, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbCameraRecognitionObject[] = { {&_swigt__p_WbCameraRecognitionObject, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbContactPoint[] = { {&_swigt__p_WbContactPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbLidarPoint[] = { {&_swigt__p_WbLidarPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbMouseState[] = { {&_swigt__p_WbMouseState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbRadarTarget[] = { {&_swigt__p_WbRadarTarget, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WheelIndex[] = { {&_swigt__p_WheelIndex, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WiperMode[] = { {&_swigt__p_WiperMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_char[] = { {&_swigt__p_char, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Car[] = { {&_swigt__p_webots__Car, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Driver[] = { {&_swigt__p_webots__Driver, 0, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Driver, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Robot[] = { {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Supervisor, _p_webots__SupervisorTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Robot, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Supervisor[] = { {&_swigt__p_webots__Supervisor, 0, 0, 0}, {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Supervisor, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Supervisor, 0, 0},{0, 0, 0, 0}}; + +static swig_cast_info *swig_cast_initial[] = { + _swigc__p_ControlMode, + _swigc__p_CoordinateSystem, + _swigc__p_EngineType, + _swigc__p_IndicatorState, + _swigc__p_Mode, + _swigc__p_SimulationMode, + _swigc__p_Type, + _swigc__p_UserInputEvent, + _swigc__p_WbCameraRecognitionObject, + _swigc__p_WbContactPoint, + _swigc__p_WbLidarPoint, + _swigc__p_WbMouseState, + _swigc__p_WbRadarTarget, + _swigc__p_WheelIndex, + _swigc__p_WiperMode, + _swigc__p_char, + _swigc__p_webots__Car, + _swigc__p_webots__Driver, + _swigc__p_webots__Robot, + _swigc__p_webots__Supervisor, +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (END) -------- */ + +static swig_const_info swig_const_table[] = { +{0, 0, 0, 0.0, 0, 0}}; + +#ifdef __cplusplus +} +#endif +/* ----------------------------------------------------------------------------- + * Type initialization: + * This problem is tough by the requirement that no dynamic + * memory is used. Also, since swig_type_info structures store pointers to + * swig_cast_info structures and swig_cast_info structures store pointers back + * to swig_type_info structures, we need some lookup code at initialization. + * The idea is that swig generates all the structures that are needed. + * The runtime then collects these partially filled structures. + * The SWIG_InitializeModule function takes these initial arrays out of + * swig_module, and does all the lookup, filling in the swig_module.types + * array with the correct data and linking the correct swig_cast_info + * structures together. + * + * The generated swig_type_info structures are assigned statically to an initial + * array. We just loop through that array, and handle each type individually. + * First we lookup if this type has been already loaded, and if so, use the + * loaded structure instead of the generated one. Then we have to fill in the + * cast linked list. The cast data is initially stored in something like a + * two-dimensional array. Each row corresponds to a type (there are the same + * number of rows as there are in the swig_type_initial array). Each entry in + * a column is one of the swig_cast_info structures for that type. + * The cast_initial array is actually an array of arrays, because each row has + * a variable number of columns. So to actually build the cast linked list, + * we find the array of casts associated with the type, and loop through it + * adding the casts to the list. The one last trick we need to do is making + * sure the type pointer in the swig_cast_info struct is correct. + * + * First off, we lookup the cast->type name to see if it is already loaded. + * There are three cases to handle: + * 1) If the cast->type has already been loaded AND the type we are adding + * casting info to has not been loaded (it is in this module), THEN we + * replace the cast->type pointer with the type pointer that has already + * been loaded. + * 2) If BOTH types (the one we are adding casting info to, and the + * cast->type) are loaded, THEN the cast info has already been loaded by + * the previous module so we just ignore it. + * 3) Finally, if cast->type has not already been loaded, then we add that + * swig_cast_info to the linked list (because the cast->type) pointer will + * be correct. + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#if 0 +} /* c-mode */ +#endif +#endif + +#if 0 +#define SWIGRUNTIME_DEBUG +#endif + + +SWIGRUNTIME void +SWIG_InitializeModule(void *clientdata) { + size_t i; + swig_module_info *module_head, *iter; + int init; + + /* check to see if the circular list has been setup, if not, set it up */ + if (swig_module.next==0) { + /* Initialize the swig_module */ + swig_module.type_initial = swig_type_initial; + swig_module.cast_initial = swig_cast_initial; + swig_module.next = &swig_module; + init = 1; + } else { + init = 0; + } + + /* Try and load any already created modules */ + module_head = SWIG_GetModule(clientdata); + if (!module_head) { + /* This is the first module loaded for this interpreter */ + /* so set the swig module into the interpreter */ + SWIG_SetModule(clientdata, &swig_module); + } else { + /* the interpreter has loaded a SWIG module, but has it loaded this one? */ + iter=module_head; + do { + if (iter==&swig_module) { + /* Our module is already in the list, so there's nothing more to do. */ + return; + } + iter=iter->next; + } while (iter!= module_head); + + /* otherwise we must add our module into the list */ + swig_module.next = module_head->next; + module_head->next = &swig_module; + } + + /* When multiple interpreters are used, a module could have already been initialized in + a different interpreter, but not yet have a pointer in this interpreter. + In this case, we do not want to continue adding types... everything should be + set up already */ + if (init == 0) return; + + /* Now work on filling in swig_module.types */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: size %lu\n", (unsigned long)swig_module.size); +#endif + for (i = 0; i < swig_module.size; ++i) { + swig_type_info *type = 0; + swig_type_info *ret; + swig_cast_info *cast; + +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); +#endif + + /* if there is another module already loaded */ + if (swig_module.next != &swig_module) { + type = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, swig_module.type_initial[i]->name); + } + if (type) { + /* Overwrite clientdata field */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found type %s\n", type->name); +#endif + if (swig_module.type_initial[i]->clientdata) { + type->clientdata = swig_module.type_initial[i]->clientdata; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found and overwrite type %s \n", type->name); +#endif + } + } else { + type = swig_module.type_initial[i]; + } + + /* Insert casting types */ + cast = swig_module.cast_initial[i]; + while (cast->type) { + /* Don't need to add information already in the list */ + ret = 0; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: look cast %s\n", cast->type->name); +#endif + if (swig_module.next != &swig_module) { + ret = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, cast->type->name); +#ifdef SWIGRUNTIME_DEBUG + if (ret) printf("SWIG_InitializeModule: found cast %s\n", ret->name); +#endif + } + if (ret) { + if (type == swig_module.type_initial[i]) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: skip old type %s\n", ret->name); +#endif + cast->type = ret; + ret = 0; + } else { + /* Check for casting already in the list */ + swig_cast_info *ocast = SWIG_TypeCheck(ret->name, type); +#ifdef SWIGRUNTIME_DEBUG + if (ocast) printf("SWIG_InitializeModule: skip old cast %s\n", ret->name); +#endif + if (!ocast) ret = 0; + } + } + + if (!ret) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: adding cast %s\n", cast->type->name); +#endif + if (type->cast) { + type->cast->prev = cast; + cast->next = type->cast; + } + type->cast = cast; + } + cast++; + } + /* Set entry in modules->types array equal to the type */ + swig_module.types[i] = type; + } + swig_module.types[i] = 0; + +#ifdef SWIGRUNTIME_DEBUG + printf("**** SWIG_InitializeModule: Cast List ******\n"); + for (i = 0; i < swig_module.size; ++i) { + int j = 0; + swig_cast_info *cast = swig_module.cast_initial[i]; + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); + while (cast->type) { + printf("SWIG_InitializeModule: cast type %s\n", cast->type->name); + cast++; + ++j; + } + printf("---- Total casts: %d\n",j); + } + printf("**** SWIG_InitializeModule: Cast List ******\n"); +#endif +} + +/* This function will propagate the clientdata field of type to +* any new swig_type_info structures that have been added into the list +* of equivalent types. It is like calling +* SWIG_TypeClientData(type, clientdata) a second time. +*/ +SWIGRUNTIME void +SWIG_PropagateClientData(void) { + size_t i; + swig_cast_info *equiv; + static int init_run = 0; + + if (init_run) return; + init_run = 1; + + for (i = 0; i < swig_module.size; i++) { + if (swig_module.types[i]->clientdata) { + equiv = swig_module.types[i]->cast; + while (equiv) { + if (!equiv->converter) { + if (equiv->type && !equiv->type->clientdata) + SWIG_TypeClientData(equiv->type, swig_module.types[i]->clientdata); + } + equiv = equiv->next; + } + } + } +} + +#ifdef __cplusplus +#if 0 +{ + /* c-mode */ +#endif +} +#endif + + + +#ifdef __cplusplus +extern "C" { +#endif + + /* Python-specific SWIG API */ +#define SWIG_newvarlink() SWIG_Python_newvarlink() +#define SWIG_addvarlink(p, name, get_attr, set_attr) SWIG_Python_addvarlink(p, name, get_attr, set_attr) +#define SWIG_InstallConstants(d, constants) SWIG_Python_InstallConstants(d, constants) + + /* ----------------------------------------------------------------------------- + * global variable support code. + * ----------------------------------------------------------------------------- */ + + typedef struct swig_globalvar { + char *name; /* Name of global variable */ + PyObject *(*get_attr)(void); /* Return the current value */ + int (*set_attr)(PyObject *); /* Set the value */ + struct swig_globalvar *next; + } swig_globalvar; + + typedef struct swig_varlinkobject { + PyObject_HEAD + swig_globalvar *vars; + } swig_varlinkobject; + + SWIGINTERN PyObject * + swig_varlink_repr(swig_varlinkobject *SWIGUNUSEDPARM(v)) { +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_InternFromString(""); +#else + return PyString_FromString(""); +#endif + } + + SWIGINTERN PyObject * + swig_varlink_str(swig_varlinkobject *v) { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *str = PyUnicode_InternFromString("("); + PyObject *tail; + PyObject *joined; + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + tail = PyUnicode_FromString(var->name); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + if (var->next) { + tail = PyUnicode_InternFromString(", "); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + } + } + tail = PyUnicode_InternFromString(")"); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; +#else + PyObject *str = PyString_FromString("("); + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + PyString_ConcatAndDel(&str,PyString_FromString(var->name)); + if (var->next) PyString_ConcatAndDel(&str,PyString_FromString(", ")); + } + PyString_ConcatAndDel(&str,PyString_FromString(")")); +#endif + return str; + } + + SWIGINTERN void + swig_varlink_dealloc(swig_varlinkobject *v) { + swig_globalvar *var = v->vars; + while (var) { + swig_globalvar *n = var->next; + free(var->name); + free(var); + var = n; + } + } + + SWIGINTERN PyObject * + swig_varlink_getattr(swig_varlinkobject *v, char *n) { + PyObject *res = NULL; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->get_attr)(); + break; + } + var = var->next; + } + if (res == NULL && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN int + swig_varlink_setattr(swig_varlinkobject *v, char *n, PyObject *p) { + int res = 1; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->set_attr)(p); + break; + } + var = var->next; + } + if (res == 1 && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN PyTypeObject* + swig_varlink_type(void) { + static char varlink__doc__[] = "Swig var link object"; + static PyTypeObject varlink_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "swigvarlink", /* tp_name */ + sizeof(swig_varlinkobject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor) swig_varlink_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc) swig_varlink_getattr, /* tp_getattr */ + (setattrfunc) swig_varlink_setattr, /* tp_setattr */ + 0, /* tp_compare */ + (reprfunc) swig_varlink_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + 0, /* tp_hash */ + 0, /* tp_call */ + (reprfunc) swig_varlink_str, /* tp_str */ + 0, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + 0, /* tp_flags */ + varlink__doc__, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, /* tp_iter -> tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + varlink_type = tmp; + type_init = 1; + if (PyType_Ready(&varlink_type) < 0) + return NULL; + } + return &varlink_type; + } + + /* Create a variable linking object for use later */ + SWIGINTERN PyObject * + SWIG_Python_newvarlink(void) { + swig_varlinkobject *result = PyObject_NEW(swig_varlinkobject, swig_varlink_type()); + if (result) { + result->vars = 0; + } + return ((PyObject*) result); + } + + SWIGINTERN void + SWIG_Python_addvarlink(PyObject *p, const char *name, PyObject *(*get_attr)(void), int (*set_attr)(PyObject *p)) { + swig_varlinkobject *v = (swig_varlinkobject *) p; + swig_globalvar *gv = (swig_globalvar *) malloc(sizeof(swig_globalvar)); + if (gv) { + size_t size = strlen(name)+1; + gv->name = (char *)malloc(size); + if (gv->name) { + memcpy(gv->name, name, size); + gv->get_attr = get_attr; + gv->set_attr = set_attr; + gv->next = v->vars; + } + } + v->vars = gv; + } + + SWIGINTERN PyObject * + SWIG_globals(void) { + static PyObject *globals = 0; + if (!globals) { + globals = SWIG_newvarlink(); + } + return globals; + } + + /* ----------------------------------------------------------------------------- + * constants/methods manipulation + * ----------------------------------------------------------------------------- */ + + /* Install Constants */ + SWIGINTERN void + SWIG_Python_InstallConstants(PyObject *d, swig_const_info constants[]) { + PyObject *obj = 0; + size_t i; + for (i = 0; constants[i].type; ++i) { + switch(constants[i].type) { + case SWIG_PY_POINTER: + obj = SWIG_InternalNewPointerObj(constants[i].pvalue, *(constants[i]).ptype,0); + break; + case SWIG_PY_BINARY: + obj = SWIG_NewPackedObj(constants[i].pvalue, constants[i].lvalue, *(constants[i].ptype)); + break; + default: + obj = 0; + break; + } + if (obj) { + PyDict_SetItemString(d, constants[i].name, obj); + Py_DECREF(obj); + } + } + } + + /* -----------------------------------------------------------------------------*/ + /* Fix SwigMethods to carry the callback ptrs when needed */ + /* -----------------------------------------------------------------------------*/ + + SWIGINTERN void + SWIG_Python_FixMethods(PyMethodDef *methods, + swig_const_info *const_table, + swig_type_info **types, + swig_type_info **types_initial) { + size_t i; + for (i = 0; methods[i].ml_name; ++i) { + const char *c = methods[i].ml_doc; + if (!c) continue; + c = strstr(c, "swig_ptr: "); + if (c) { + int j; + swig_const_info *ci = 0; + const char *name = c + 10; + for (j = 0; const_table[j].type; ++j) { + if (strncmp(const_table[j].name, name, + strlen(const_table[j].name)) == 0) { + ci = &(const_table[j]); + break; + } + } + if (ci) { + void *ptr = (ci->type == SWIG_PY_POINTER) ? ci->pvalue : 0; + if (ptr) { + size_t shift = (ci->ptype) - types; + swig_type_info *ty = types_initial[shift]; + size_t ldoc = (c - methods[i].ml_doc); + size_t lptr = strlen(ty->name)+2*sizeof(void*)+2; + char *ndoc = (char*)malloc(ldoc + lptr + 10); + if (ndoc) { + char *buff = ndoc; + memcpy(buff, methods[i].ml_doc, ldoc); + buff += ldoc; + memcpy(buff, "swig_ptr: ", 10); + buff += 10; + SWIG_PackVoidPtr(buff, ptr, ty->name, lptr); + methods[i].ml_doc = ndoc; + } + } + } + } + } + } + + /* ----------------------------------------------------------------------------- + * Method creation and docstring support functions + * ----------------------------------------------------------------------------- */ + + /* ----------------------------------------------------------------------------- + * Function to find the method definition with the correct docstring for the + * proxy module as opposed to the low-level API + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name) { + /* Find the function in the modified method table */ + size_t offset = 0; + int found = 0; + while (SwigMethods_proxydocs[offset].ml_meth != NULL) { + if (strcmp(SwigMethods_proxydocs[offset].ml_name, name) == 0) { + found = 1; + break; + } + offset++; + } + /* Use the copy with the modified docstring if available */ + return found ? &SwigMethods_proxydocs[offset] : NULL; + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyInstanceMethod_New() used in Python 3 + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } +#if PY_VERSION_HEX >= 0x03000000 + return PyInstanceMethod_New(func); +#else + return PyMethod_New(func, NULL, NULL); +#endif + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyStaticMethod_New() + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } + return PyStaticMethod_New(func); + } + +#ifdef __cplusplus +} +#endif + +/* -----------------------------------------------------------------------------* + * Partial Init method + * -----------------------------------------------------------------------------*/ + +#ifdef __cplusplus +extern "C" +#endif + +SWIGEXPORT +#if PY_VERSION_HEX >= 0x03000000 +PyObject* +#else +void +#endif +SWIG_init(void) { + PyObject *m, *d, *md, *globals; + +#if PY_VERSION_HEX >= 0x03000000 + static struct PyModuleDef SWIG_module = { + PyModuleDef_HEAD_INIT, + SWIG_name, + NULL, + -1, + SwigMethods, + NULL, + NULL, + NULL, + NULL + }; +#endif + +#if defined(SWIGPYTHON_BUILTIN) + static SwigPyClientData SwigPyObject_clientdata = { + 0, 0, 0, 0, 0, 0, 0 + }; + static PyGetSetDef this_getset_def = { + (char *)"this", &SwigPyBuiltin_ThisClosure, NULL, NULL, NULL + }; + static SwigPyGetSet thisown_getset_closure = { + SwigPyObject_own, + SwigPyObject_own + }; + static PyGetSetDef thisown_getset_def = { + (char *)"thisown", SwigPyBuiltin_GetterClosure, SwigPyBuiltin_SetterClosure, NULL, &thisown_getset_closure + }; + PyTypeObject *builtin_pytype; + int builtin_base_count; + swig_type_info *builtin_basetype; + PyObject *tuple; + PyGetSetDescrObject *static_getset; + PyTypeObject *metatype; + PyTypeObject *swigpyobject; + SwigPyClientData *cd; + PyObject *public_interface, *public_symbol; + PyObject *this_descr; + PyObject *thisown_descr; + PyObject *self = 0; + int i; + + (void)builtin_pytype; + (void)builtin_base_count; + (void)builtin_basetype; + (void)tuple; + (void)static_getset; + (void)self; + + /* Metaclass is used to implement static member variables */ + metatype = SwigPyObjectType(); + assert(metatype); +#endif + + (void)globals; + + /* Create singletons now to avoid potential deadlocks with multi-threaded usage after module initialization */ + SWIG_This(); + SWIG_Python_TypeCache(); + SwigPyPacked_type(); +#ifndef SWIGPYTHON_BUILTIN + SwigPyObject_type(); +#endif + + /* Fix SwigMethods to carry the callback ptrs when needed */ + SWIG_Python_FixMethods(SwigMethods, swig_const_table, swig_types, swig_type_initial); + +#if PY_VERSION_HEX >= 0x03000000 + m = PyModule_Create(&SWIG_module); +#else + m = Py_InitModule(SWIG_name, SwigMethods); +#endif + + md = d = PyModule_GetDict(m); + (void)md; + + SWIG_InitializeModule(0); + +#ifdef SWIGPYTHON_BUILTIN + swigpyobject = SwigPyObject_TypeOnce(); + + SwigPyObject_stype = SWIG_MangledTypeQuery("_p_SwigPyObject"); + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + if (!cd) { + SwigPyObject_stype->clientdata = &SwigPyObject_clientdata; + SwigPyObject_clientdata.pytype = swigpyobject; + } else if (swigpyobject->tp_basicsize != cd->pytype->tp_basicsize) { + PyErr_SetString(PyExc_RuntimeError, "Import error: attempted to load two incompatible swig-generated modules."); +# if PY_VERSION_HEX >= 0x03000000 + return NULL; +# else + return; +# endif + } + + /* All objects have a 'this' attribute */ + this_descr = PyDescr_NewGetSet(SwigPyObject_type(), &this_getset_def); + (void)this_descr; + + /* All objects have a 'thisown' attribute */ + thisown_descr = PyDescr_NewGetSet(SwigPyObject_type(), &thisown_getset_def); + (void)thisown_descr; + + public_interface = PyList_New(0); + public_symbol = 0; + (void)public_symbol; + + PyDict_SetItemString(md, "__all__", public_interface); + Py_DECREF(public_interface); + for (i = 0; SwigMethods[i].ml_name != NULL; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, SwigMethods[i].ml_name); + for (i = 0; swig_const_table[i].name != 0; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, swig_const_table[i].name); +#endif + + SWIG_InstallConstants(d,swig_const_table); + + SWIG_Python_SetConstant(d, "Driver_INDICATOR_OFF",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_OFF))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_RIGHT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_RIGHT))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_LEFT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_LEFT))); + SWIG_Python_SetConstant(d, "Driver_SPEED",SWIG_From_int(static_cast< int >(webots::Driver::SPEED))); + SWIG_Python_SetConstant(d, "Driver_TORQUE",SWIG_From_int(static_cast< int >(webots::Driver::TORQUE))); + SWIG_Python_SetConstant(d, "Driver_DOWN",SWIG_From_int(static_cast< int >(webots::Driver::DOWN))); + SWIG_Python_SetConstant(d, "Driver_SLOW",SWIG_From_int(static_cast< int >(webots::Driver::SLOW))); + SWIG_Python_SetConstant(d, "Driver_NORMAL",SWIG_From_int(static_cast< int >(webots::Driver::NORMAL))); + SWIG_Python_SetConstant(d, "Driver_FAST",SWIG_From_int(static_cast< int >(webots::Driver::FAST))); + SWIG_Python_SetConstant(d, "Car_TRACTION",SWIG_From_int(static_cast< int >(webots::Car::TRACTION))); + SWIG_Python_SetConstant(d, "Car_PROPULSION",SWIG_From_int(static_cast< int >(webots::Car::PROPULSION))); + SWIG_Python_SetConstant(d, "Car_FOUR_BY_FOUR",SWIG_From_int(static_cast< int >(webots::Car::FOUR_BY_FOUR))); + SWIG_Python_SetConstant(d, "Car_COMBUSTION_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::COMBUSTION_ENGINE))); + SWIG_Python_SetConstant(d, "Car_ELECTRIC_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::ELECTRIC_ENGINE))); + SWIG_Python_SetConstant(d, "Car_PARALLEL_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::PARALLEL_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_POWER_SPLIT_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::POWER_SPLIT_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_NB",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_NB))); +#if PY_VERSION_HEX >= 0x03000000 + return m; +#else + return; +#endif +} + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle38.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle38.cpp new file mode 100644 index 000000000..cbfd22412 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle38.cpp @@ -0,0 +1,5255 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 4.0.2 + * + * This file is not intended to be easily readable and contains a number of + * coding conventions designed to improve portability and efficiency. Do not make + * changes to this file unless you know what you are doing--modify the SWIG + * interface file instead. + * ----------------------------------------------------------------------------- */ + + +#ifndef SWIGPYTHON +#define SWIGPYTHON +#endif + +#define SWIG_PYTHON_DIRECTOR_NO_VTABLE + + +#ifdef __cplusplus +/* SwigValueWrapper is described in swig.swg */ +template class SwigValueWrapper { + struct SwigMovePointer { + T *ptr; + SwigMovePointer(T *p) : ptr(p) { } + ~SwigMovePointer() { delete ptr; } + SwigMovePointer& operator=(SwigMovePointer& rhs) { T* oldptr = ptr; ptr = 0; delete oldptr; ptr = rhs.ptr; rhs.ptr = 0; return *this; } + } pointer; + SwigValueWrapper& operator=(const SwigValueWrapper& rhs); + SwigValueWrapper(const SwigValueWrapper& rhs); +public: + SwigValueWrapper() : pointer(0) { } + SwigValueWrapper& operator=(const T& t) { SwigMovePointer tmp(new T(t)); pointer = tmp; return *this; } + operator T&() const { return *pointer.ptr; } + T *operator&() { return pointer.ptr; } +}; + +template T SwigValueInit() { + return T(); +} +#endif + +/* ----------------------------------------------------------------------------- + * This section contains generic SWIG labels for method/variable + * declarations/attributes, and other compiler dependent labels. + * ----------------------------------------------------------------------------- */ + +/* template workaround for compilers that cannot correctly implement the C++ standard */ +#ifndef SWIGTEMPLATEDISAMBIGUATOR +# if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x560) +# define SWIGTEMPLATEDISAMBIGUATOR template +# elif defined(__HP_aCC) +/* Needed even with `aCC -AA' when `aCC -V' reports HP ANSI C++ B3910B A.03.55 */ +/* If we find a maximum version that requires this, the test would be __HP_aCC <= 35500 for A.03.55 */ +# define SWIGTEMPLATEDISAMBIGUATOR template +# else +# define SWIGTEMPLATEDISAMBIGUATOR +# endif +#endif + +/* inline attribute */ +#ifndef SWIGINLINE +# if defined(__cplusplus) || (defined(__GNUC__) && !defined(__STRICT_ANSI__)) +# define SWIGINLINE inline +# else +# define SWIGINLINE +# endif +#endif + +/* attribute recognised by some compilers to avoid 'unused' warnings */ +#ifndef SWIGUNUSED +# if defined(__GNUC__) +# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +# elif defined(__ICC) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +#endif + +#ifndef SWIG_MSC_UNSUPPRESS_4505 +# if defined(_MSC_VER) +# pragma warning(disable : 4505) /* unreferenced local function has been removed */ +# endif +#endif + +#ifndef SWIGUNUSEDPARM +# ifdef __cplusplus +# define SWIGUNUSEDPARM(p) +# else +# define SWIGUNUSEDPARM(p) p SWIGUNUSED +# endif +#endif + +/* internal SWIG method */ +#ifndef SWIGINTERN +# define SWIGINTERN static SWIGUNUSED +#endif + +/* internal inline SWIG method */ +#ifndef SWIGINTERNINLINE +# define SWIGINTERNINLINE SWIGINTERN SWIGINLINE +#endif + +/* exporting methods */ +#if defined(__GNUC__) +# if (__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) +# ifndef GCC_HASCLASSVISIBILITY +# define GCC_HASCLASSVISIBILITY +# endif +# endif +#endif + +#ifndef SWIGEXPORT +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# if defined(STATIC_LINKED) +# define SWIGEXPORT +# else +# define SWIGEXPORT __declspec(dllexport) +# endif +# else +# if defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) +# define SWIGEXPORT __attribute__ ((visibility("default"))) +# else +# define SWIGEXPORT +# endif +# endif +#endif + +/* calling conventions for Windows */ +#ifndef SWIGSTDCALL +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# define SWIGSTDCALL __stdcall +# else +# define SWIGSTDCALL +# endif +#endif + +/* Deal with Microsoft's attempt at deprecating C standard runtime functions */ +#if !defined(SWIG_NO_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_CRT_SECURE_NO_DEPRECATE) +# define _CRT_SECURE_NO_DEPRECATE +#endif + +/* Deal with Microsoft's attempt at deprecating methods in the standard C++ library */ +#if !defined(SWIG_NO_SCL_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_SCL_SECURE_NO_DEPRECATE) +# define _SCL_SECURE_NO_DEPRECATE +#endif + +/* Deal with Apple's deprecated 'AssertMacros.h' from Carbon-framework */ +#if defined(__APPLE__) && !defined(__ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES) +# define __ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES 0 +#endif + +/* Intel's compiler complains if a variable which was never initialised is + * cast to void, which is a common idiom which we use to indicate that we + * are aware a variable isn't used. So we just silence that warning. + * See: https://github.com/swig/swig/issues/192 for more discussion. + */ +#ifdef __INTEL_COMPILER +# pragma warning disable 592 +#endif + + +#if defined(__GNUC__) && defined(_WIN32) && !defined(SWIG_PYTHON_NO_HYPOT_WORKAROUND) +/* Workaround for '::hypot' has not been declared', see https://bugs.python.org/issue11566 */ +# include +#endif + +#if defined(_DEBUG) && defined(SWIG_PYTHON_INTERPRETER_NO_DEBUG) +/* Use debug wrappers with the Python release dll */ +# undef _DEBUG +# include +# define _DEBUG 1 +#else +# include +#endif + +/* ----------------------------------------------------------------------------- + * swigrun.swg + * + * This file contains generic C API SWIG runtime support for pointer + * type checking. + * ----------------------------------------------------------------------------- */ + +/* This should only be incremented when either the layout of swig_type_info changes, + or for whatever reason, the runtime changes incompatibly */ +#define SWIG_RUNTIME_VERSION "4" + +/* define SWIG_TYPE_TABLE_NAME as "SWIG_TYPE_TABLE" */ +#ifdef SWIG_TYPE_TABLE +# define SWIG_QUOTE_STRING(x) #x +# define SWIG_EXPAND_AND_QUOTE_STRING(x) SWIG_QUOTE_STRING(x) +# define SWIG_TYPE_TABLE_NAME SWIG_EXPAND_AND_QUOTE_STRING(SWIG_TYPE_TABLE) +#else +# define SWIG_TYPE_TABLE_NAME +#endif + +/* + You can use the SWIGRUNTIME and SWIGRUNTIMEINLINE macros for + creating a static or dynamic library from the SWIG runtime code. + In 99.9% of the cases, SWIG just needs to declare them as 'static'. + + But only do this if strictly necessary, ie, if you have problems + with your compiler or suchlike. +*/ + +#ifndef SWIGRUNTIME +# define SWIGRUNTIME SWIGINTERN +#endif + +#ifndef SWIGRUNTIMEINLINE +# define SWIGRUNTIMEINLINE SWIGRUNTIME SWIGINLINE +#endif + +/* Generic buffer size */ +#ifndef SWIG_BUFFER_SIZE +# define SWIG_BUFFER_SIZE 1024 +#endif + +/* Flags for pointer conversions */ +#define SWIG_POINTER_DISOWN 0x1 +#define SWIG_CAST_NEW_MEMORY 0x2 +#define SWIG_POINTER_NO_NULL 0x4 + +/* Flags for new pointer objects */ +#define SWIG_POINTER_OWN 0x1 + + +/* + Flags/methods for returning states. + + The SWIG conversion methods, as ConvertPtr, return an integer + that tells if the conversion was successful or not. And if not, + an error code can be returned (see swigerrors.swg for the codes). + + Use the following macros/flags to set or process the returning + states. + + In old versions of SWIG, code such as the following was usually written: + + if (SWIG_ConvertPtr(obj,vptr,ty.flags) != -1) { + // success code + } else { + //fail code + } + + Now you can be more explicit: + + int res = SWIG_ConvertPtr(obj,vptr,ty.flags); + if (SWIG_IsOK(res)) { + // success code + } else { + // fail code + } + + which is the same really, but now you can also do + + Type *ptr; + int res = SWIG_ConvertPtr(obj,(void **)(&ptr),ty.flags); + if (SWIG_IsOK(res)) { + // success code + if (SWIG_IsNewObj(res) { + ... + delete *ptr; + } else { + ... + } + } else { + // fail code + } + + I.e., now SWIG_ConvertPtr can return new objects and you can + identify the case and take care of the deallocation. Of course that + also requires SWIG_ConvertPtr to return new result values, such as + + int SWIG_ConvertPtr(obj, ptr,...) { + if () { + if () { + *ptr = ; + return SWIG_NEWOBJ; + } else { + *ptr = ; + return SWIG_OLDOBJ; + } + } else { + return SWIG_BADOBJ; + } + } + + Of course, returning the plain '0(success)/-1(fail)' still works, but you can be + more explicit by returning SWIG_BADOBJ, SWIG_ERROR or any of the + SWIG errors code. + + Finally, if the SWIG_CASTRANK_MODE is enabled, the result code + allows to return the 'cast rank', for example, if you have this + + int food(double) + int fooi(int); + + and you call + + food(1) // cast rank '1' (1 -> 1.0) + fooi(1) // cast rank '0' + + just use the SWIG_AddCast()/SWIG_CheckState() +*/ + +#define SWIG_OK (0) +#define SWIG_ERROR (-1) +#define SWIG_IsOK(r) (r >= 0) +#define SWIG_ArgError(r) ((r != SWIG_ERROR) ? r : SWIG_TypeError) + +/* The CastRankLimit says how many bits are used for the cast rank */ +#define SWIG_CASTRANKLIMIT (1 << 8) +/* The NewMask denotes the object was created (using new/malloc) */ +#define SWIG_NEWOBJMASK (SWIG_CASTRANKLIMIT << 1) +/* The TmpMask is for in/out typemaps that use temporal objects */ +#define SWIG_TMPOBJMASK (SWIG_NEWOBJMASK << 1) +/* Simple returning values */ +#define SWIG_BADOBJ (SWIG_ERROR) +#define SWIG_OLDOBJ (SWIG_OK) +#define SWIG_NEWOBJ (SWIG_OK | SWIG_NEWOBJMASK) +#define SWIG_TMPOBJ (SWIG_OK | SWIG_TMPOBJMASK) +/* Check, add and del mask methods */ +#define SWIG_AddNewMask(r) (SWIG_IsOK(r) ? (r | SWIG_NEWOBJMASK) : r) +#define SWIG_DelNewMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_NEWOBJMASK) : r) +#define SWIG_IsNewObj(r) (SWIG_IsOK(r) && (r & SWIG_NEWOBJMASK)) +#define SWIG_AddTmpMask(r) (SWIG_IsOK(r) ? (r | SWIG_TMPOBJMASK) : r) +#define SWIG_DelTmpMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_TMPOBJMASK) : r) +#define SWIG_IsTmpObj(r) (SWIG_IsOK(r) && (r & SWIG_TMPOBJMASK)) + +/* Cast-Rank Mode */ +#if defined(SWIG_CASTRANK_MODE) +# ifndef SWIG_TypeRank +# define SWIG_TypeRank unsigned long +# endif +# ifndef SWIG_MAXCASTRANK /* Default cast allowed */ +# define SWIG_MAXCASTRANK (2) +# endif +# define SWIG_CASTRANKMASK ((SWIG_CASTRANKLIMIT) -1) +# define SWIG_CastRank(r) (r & SWIG_CASTRANKMASK) +SWIGINTERNINLINE int SWIG_AddCast(int r) { + return SWIG_IsOK(r) ? ((SWIG_CastRank(r) < SWIG_MAXCASTRANK) ? (r + 1) : SWIG_ERROR) : r; +} +SWIGINTERNINLINE int SWIG_CheckState(int r) { + return SWIG_IsOK(r) ? SWIG_CastRank(r) + 1 : 0; +} +#else /* no cast-rank mode */ +# define SWIG_AddCast(r) (r) +# define SWIG_CheckState(r) (SWIG_IsOK(r) ? 1 : 0) +#endif + + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void *(*swig_converter_func)(void *, int *); +typedef struct swig_type_info *(*swig_dycast_func)(void **); + +/* Structure to store information on one type */ +typedef struct swig_type_info { + const char *name; /* mangled name of this type */ + const char *str; /* human readable name of this type */ + swig_dycast_func dcast; /* dynamic cast function down a hierarchy */ + struct swig_cast_info *cast; /* linked list of types that can cast into this type */ + void *clientdata; /* language specific type data */ + int owndata; /* flag if the structure owns the clientdata */ +} swig_type_info; + +/* Structure to store a type and conversion function used for casting */ +typedef struct swig_cast_info { + swig_type_info *type; /* pointer to type that is equivalent to this type */ + swig_converter_func converter; /* function to cast the void pointers */ + struct swig_cast_info *next; /* pointer to next cast in linked list */ + struct swig_cast_info *prev; /* pointer to the previous cast */ +} swig_cast_info; + +/* Structure used to store module information + * Each module generates one structure like this, and the runtime collects + * all of these structures and stores them in a circularly linked list.*/ +typedef struct swig_module_info { + swig_type_info **types; /* Array of pointers to swig_type_info structures that are in this module */ + size_t size; /* Number of types in this module */ + struct swig_module_info *next; /* Pointer to next element in circularly linked list */ + swig_type_info **type_initial; /* Array of initially generated type structures */ + swig_cast_info **cast_initial; /* Array of initially generated casting structures */ + void *clientdata; /* Language specific module data */ +} swig_module_info; + +/* + Compare two type names skipping the space characters, therefore + "char*" == "char *" and "Class" == "Class", etc. + + Return 0 when the two name types are equivalent, as in + strncmp, but skipping ' '. +*/ +SWIGRUNTIME int +SWIG_TypeNameComp(const char *f1, const char *l1, + const char *f2, const char *l2) { + for (;(f1 != l1) && (f2 != l2); ++f1, ++f2) { + while ((*f1 == ' ') && (f1 != l1)) ++f1; + while ((*f2 == ' ') && (f2 != l2)) ++f2; + if (*f1 != *f2) return (*f1 > *f2) ? 1 : -1; + } + return (int)((l1 - f1) - (l2 - f2)); +} + +/* + Check type equivalence in a name list like ||... + Return 0 if equal, -1 if nb < tb, 1 if nb > tb +*/ +SWIGRUNTIME int +SWIG_TypeCmp(const char *nb, const char *tb) { + int equiv = 1; + const char* te = tb + strlen(tb); + const char* ne = nb; + while (equiv != 0 && *ne) { + for (nb = ne; *ne; ++ne) { + if (*ne == '|') break; + } + equiv = SWIG_TypeNameComp(nb, ne, tb, te); + if (*ne) ++ne; + } + return equiv; +} + +/* + Check type equivalence in a name list like ||... + Return 0 if not equal, 1 if equal +*/ +SWIGRUNTIME int +SWIG_TypeEquiv(const char *nb, const char *tb) { + return SWIG_TypeCmp(nb, tb) == 0 ? 1 : 0; +} + +/* + Check the typename +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheck(const char *c, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (strcmp(iter->type->name, c) == 0) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Identical to SWIG_TypeCheck, except strcmp is replaced with a pointer comparison +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheckStruct(swig_type_info *from, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (iter->type == from) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Cast a pointer up an inheritance hierarchy +*/ +SWIGRUNTIMEINLINE void * +SWIG_TypeCast(swig_cast_info *ty, void *ptr, int *newmemory) { + return ((!ty) || (!ty->converter)) ? ptr : (*ty->converter)(ptr, newmemory); +} + +/* + Dynamic pointer casting. Down an inheritance hierarchy +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeDynamicCast(swig_type_info *ty, void **ptr) { + swig_type_info *lastty = ty; + if (!ty || !ty->dcast) return ty; + while (ty && (ty->dcast)) { + ty = (*ty->dcast)(ptr); + if (ty) lastty = ty; + } + return lastty; +} + +/* + Return the name associated with this type +*/ +SWIGRUNTIMEINLINE const char * +SWIG_TypeName(const swig_type_info *ty) { + return ty->name; +} + +/* + Return the pretty name associated with this type, + that is an unmangled type name in a form presentable to the user. +*/ +SWIGRUNTIME const char * +SWIG_TypePrettyName(const swig_type_info *type) { + /* The "str" field contains the equivalent pretty names of the + type, separated by vertical-bar characters. We choose + to print the last name, as it is often (?) the most + specific. */ + if (!type) return NULL; + if (type->str != NULL) { + const char *last_name = type->str; + const char *s; + for (s = type->str; *s; s++) + if (*s == '|') last_name = s+1; + return last_name; + } + else + return type->name; +} + +/* + Set the clientdata field for a type +*/ +SWIGRUNTIME void +SWIG_TypeClientData(swig_type_info *ti, void *clientdata) { + swig_cast_info *cast = ti->cast; + /* if (ti->clientdata == clientdata) return; */ + ti->clientdata = clientdata; + + while (cast) { + if (!cast->converter) { + swig_type_info *tc = cast->type; + if (!tc->clientdata) { + SWIG_TypeClientData(tc, clientdata); + } + } + cast = cast->next; + } +} +SWIGRUNTIME void +SWIG_TypeNewClientData(swig_type_info *ti, void *clientdata) { + SWIG_TypeClientData(ti, clientdata); + ti->owndata = 1; +} + +/* + Search for a swig_type_info structure only by mangled name + Search is a O(log #types) + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_MangledTypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + swig_module_info *iter = start; + do { + if (iter->size) { + size_t l = 0; + size_t r = iter->size - 1; + do { + /* since l+r >= 0, we can (>> 1) instead (/ 2) */ + size_t i = (l + r) >> 1; + const char *iname = iter->types[i]->name; + if (iname) { + int compare = strcmp(name, iname); + if (compare == 0) { + return iter->types[i]; + } else if (compare < 0) { + if (i) { + r = i - 1; + } else { + break; + } + } else if (compare > 0) { + l = i + 1; + } + } else { + break; /* should never happen */ + } + } while (l <= r); + } + iter = iter->next; + } while (iter != end); + return 0; +} + +/* + Search for a swig_type_info structure for either a mangled name or a human readable name. + It first searches the mangled names of the types, which is a O(log #types) + If a type is not found it then searches the human readable names, which is O(#types). + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + /* STEP 1: Search the name field using binary search */ + swig_type_info *ret = SWIG_MangledTypeQueryModule(start, end, name); + if (ret) { + return ret; + } else { + /* STEP 2: If the type hasn't been found, do a complete search + of the str field (the human readable name) */ + swig_module_info *iter = start; + do { + size_t i = 0; + for (; i < iter->size; ++i) { + if (iter->types[i]->str && (SWIG_TypeEquiv(iter->types[i]->str, name))) + return iter->types[i]; + } + iter = iter->next; + } while (iter != end); + } + + /* neither found a match */ + return 0; +} + +/* + Pack binary data into a string +*/ +SWIGRUNTIME char * +SWIG_PackData(char *c, void *ptr, size_t sz) { + static const char hex[17] = "0123456789abcdef"; + const unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + unsigned char uu = *u; + *(c++) = hex[(uu & 0xf0) >> 4]; + *(c++) = hex[uu & 0xf]; + } + return c; +} + +/* + Unpack binary data from a string +*/ +SWIGRUNTIME const char * +SWIG_UnpackData(const char *c, void *ptr, size_t sz) { + unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + char d = *(c++); + unsigned char uu; + if ((d >= '0') && (d <= '9')) + uu = (unsigned char)((d - '0') << 4); + else if ((d >= 'a') && (d <= 'f')) + uu = (unsigned char)((d - ('a'-10)) << 4); + else + return (char *) 0; + d = *(c++); + if ((d >= '0') && (d <= '9')) + uu |= (unsigned char)(d - '0'); + else if ((d >= 'a') && (d <= 'f')) + uu |= (unsigned char)(d - ('a'-10)); + else + return (char *) 0; + *u = uu; + } + return c; +} + +/* + Pack 'void *' into a string buffer. +*/ +SWIGRUNTIME char * +SWIG_PackVoidPtr(char *buff, void *ptr, const char *name, size_t bsz) { + char *r = buff; + if ((2*sizeof(void *) + 2) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,&ptr,sizeof(void *)); + if (strlen(name) + 1 > (bsz - (r - buff))) return 0; + strcpy(r,name); + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackVoidPtr(const char *c, void **ptr, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + *ptr = (void *) 0; + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sizeof(void *)); +} + +SWIGRUNTIME char * +SWIG_PackDataName(char *buff, void *ptr, size_t sz, const char *name, size_t bsz) { + char *r = buff; + size_t lname = (name ? strlen(name) : 0); + if ((2*sz + 2 + lname) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,ptr,sz); + if (lname) { + strncpy(r,name,lname+1); + } else { + *r = 0; + } + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackDataName(const char *c, void *ptr, size_t sz, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + memset(ptr,0,sz); + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sz); +} + +#ifdef __cplusplus +} +#endif + +/* Errors in SWIG */ +#define SWIG_UnknownError -1 +#define SWIG_IOError -2 +#define SWIG_RuntimeError -3 +#define SWIG_IndexError -4 +#define SWIG_TypeError -5 +#define SWIG_DivisionByZero -6 +#define SWIG_OverflowError -7 +#define SWIG_SyntaxError -8 +#define SWIG_ValueError -9 +#define SWIG_SystemError -10 +#define SWIG_AttributeError -11 +#define SWIG_MemoryError -12 +#define SWIG_NullReferenceError -13 + + + +/* Compatibility macros for Python 3 */ +#if PY_VERSION_HEX >= 0x03000000 + +#define PyClass_Check(obj) PyObject_IsInstance(obj, (PyObject *)&PyType_Type) +#define PyInt_Check(x) PyLong_Check(x) +#define PyInt_AsLong(x) PyLong_AsLong(x) +#define PyInt_FromLong(x) PyLong_FromLong(x) +#define PyInt_FromSize_t(x) PyLong_FromSize_t(x) +#define PyString_Check(name) PyBytes_Check(name) +#define PyString_FromString(x) PyUnicode_FromString(x) +#define PyString_Format(fmt, args) PyUnicode_Format(fmt, args) +#define PyString_AsString(str) PyBytes_AsString(str) +#define PyString_Size(str) PyBytes_Size(str) +#define PyString_InternFromString(key) PyUnicode_InternFromString(key) +#define Py_TPFLAGS_HAVE_CLASS Py_TPFLAGS_BASETYPE +#define PyString_AS_STRING(x) PyUnicode_AS_STRING(x) +#define _PyLong_FromSsize_t(x) PyLong_FromSsize_t(x) + +#endif + +#ifndef Py_TYPE +# define Py_TYPE(op) ((op)->ob_type) +#endif + +/* SWIG APIs for compatibility of both Python 2 & 3 */ + +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_Python_str_FromFormat PyUnicode_FromFormat +#else +# define SWIG_Python_str_FromFormat PyString_FromFormat +#endif + + +/* Warning: This function will allocate a new string in Python 3, + * so please call SWIG_Python_str_DelForPy3(x) to free the space. + */ +SWIGINTERN char* +SWIG_Python_str_AsChar(PyObject *str) +{ +#if PY_VERSION_HEX >= 0x03030000 + return (char *)PyUnicode_AsUTF8(str); +#elif PY_VERSION_HEX >= 0x03000000 + char *newstr = 0; + str = PyUnicode_AsUTF8String(str); + if (str) { + char *cstr; + Py_ssize_t len; + if (PyBytes_AsStringAndSize(str, &cstr, &len) != -1) { + newstr = (char *) malloc(len+1); + if (newstr) + memcpy(newstr, cstr, len+1); + } + Py_XDECREF(str); + } + return newstr; +#else + return PyString_AsString(str); +#endif +} + +#if PY_VERSION_HEX >= 0x03030000 || PY_VERSION_HEX < 0x03000000 +# define SWIG_Python_str_DelForPy3(x) +#else +# define SWIG_Python_str_DelForPy3(x) free( (void*) (x) ) +#endif + + +SWIGINTERN PyObject* +SWIG_Python_str_FromChar(const char *c) +{ +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_FromString(c); +#else + return PyString_FromString(c); +#endif +} + +#ifndef PyObject_DEL +# define PyObject_DEL PyObject_Del +#endif + +// SWIGPY_USE_CAPSULE is no longer used within SWIG itself, but some user +// interface files check for it. +# define SWIGPY_USE_CAPSULE +# define SWIGPY_CAPSULE_NAME ("swig_runtime_data" SWIG_RUNTIME_VERSION ".type_pointer_capsule" SWIG_TYPE_TABLE_NAME) + +#if PY_VERSION_HEX < 0x03020000 +#define PyDescr_TYPE(x) (((PyDescrObject *)(x))->d_type) +#define PyDescr_NAME(x) (((PyDescrObject *)(x))->d_name) +#define Py_hash_t long +#endif + +/* ----------------------------------------------------------------------------- + * error manipulation + * ----------------------------------------------------------------------------- */ + +SWIGRUNTIME PyObject* +SWIG_Python_ErrorType(int code) { + PyObject* type = 0; + switch(code) { + case SWIG_MemoryError: + type = PyExc_MemoryError; + break; + case SWIG_IOError: + type = PyExc_IOError; + break; + case SWIG_RuntimeError: + type = PyExc_RuntimeError; + break; + case SWIG_IndexError: + type = PyExc_IndexError; + break; + case SWIG_TypeError: + type = PyExc_TypeError; + break; + case SWIG_DivisionByZero: + type = PyExc_ZeroDivisionError; + break; + case SWIG_OverflowError: + type = PyExc_OverflowError; + break; + case SWIG_SyntaxError: + type = PyExc_SyntaxError; + break; + case SWIG_ValueError: + type = PyExc_ValueError; + break; + case SWIG_SystemError: + type = PyExc_SystemError; + break; + case SWIG_AttributeError: + type = PyExc_AttributeError; + break; + default: + type = PyExc_RuntimeError; + } + return type; +} + + +SWIGRUNTIME void +SWIG_Python_AddErrorMsg(const char* mesg) +{ + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + + if (PyErr_Occurred()) + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + PyErr_Clear(); + Py_XINCREF(type); + if (tmp) + PyErr_Format(type, "%s %s", tmp, mesg); + else + PyErr_Format(type, "%s", mesg); + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + Py_DECREF(value); + } else { + PyErr_SetString(PyExc_RuntimeError, mesg); + } +} + +SWIGRUNTIME int +SWIG_Python_TypeErrorOccurred(PyObject *obj) +{ + PyObject *error; + if (obj) + return 0; + error = PyErr_Occurred(); + return error && PyErr_GivenExceptionMatches(error, PyExc_TypeError); +} + +SWIGRUNTIME void +SWIG_Python_RaiseOrModifyTypeError(const char *message) +{ + if (SWIG_Python_TypeErrorOccurred(NULL)) { + /* Use existing TypeError to preserve stacktrace and enhance with given message */ + PyObject *newvalue; + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); +#if PY_VERSION_HEX >= 0x03000000 + newvalue = PyUnicode_FromFormat("%S\nAdditional information:\n%s", value, message); +#else + newvalue = PyString_FromFormat("%s\nAdditional information:\n%s", PyString_AsString(value), message); +#endif + Py_XDECREF(value); + PyErr_Restore(type, newvalue, traceback); + } else { + /* Raise TypeError using given message */ + PyErr_SetString(PyExc_TypeError, message); + } +} + +#if defined(SWIG_PYTHON_NO_THREADS) +# if defined(SWIG_PYTHON_THREADS) +# undef SWIG_PYTHON_THREADS +# endif +#endif +#if defined(SWIG_PYTHON_THREADS) /* Threading support is enabled */ +# if !defined(SWIG_PYTHON_USE_GIL) && !defined(SWIG_PYTHON_NO_USE_GIL) +# define SWIG_PYTHON_USE_GIL +# endif +# if defined(SWIG_PYTHON_USE_GIL) /* Use PyGILState threads calls */ +# ifndef SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_INITIALIZE_THREADS PyEval_InitThreads() +# endif +# ifdef __cplusplus /* C++ code */ + class SWIG_Python_Thread_Block { + bool status; + PyGILState_STATE state; + public: + void end() { if (status) { PyGILState_Release(state); status = false;} } + SWIG_Python_Thread_Block() : status(true), state(PyGILState_Ensure()) {} + ~SWIG_Python_Thread_Block() { end(); } + }; + class SWIG_Python_Thread_Allow { + bool status; + PyThreadState *save; + public: + void end() { if (status) { PyEval_RestoreThread(save); status = false; }} + SWIG_Python_Thread_Allow() : status(true), save(PyEval_SaveThread()) {} + ~SWIG_Python_Thread_Allow() { end(); } + }; +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK SWIG_Python_Thread_Block _swig_thread_block +# define SWIG_PYTHON_THREAD_END_BLOCK _swig_thread_block.end() +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW SWIG_Python_Thread_Allow _swig_thread_allow +# define SWIG_PYTHON_THREAD_END_ALLOW _swig_thread_allow.end() +# else /* C code */ +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK PyGILState_STATE _swig_thread_block = PyGILState_Ensure() +# define SWIG_PYTHON_THREAD_END_BLOCK PyGILState_Release(_swig_thread_block) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW PyThreadState *_swig_thread_allow = PyEval_SaveThread() +# define SWIG_PYTHON_THREAD_END_ALLOW PyEval_RestoreThread(_swig_thread_allow) +# endif +# else /* Old thread way, not implemented, user must provide it */ +# if !defined(SWIG_PYTHON_INITIALIZE_THREADS) +# define SWIG_PYTHON_INITIALIZE_THREADS +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_BLOCK) +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_END_BLOCK) +# define SWIG_PYTHON_THREAD_END_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_ALLOW) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# endif +# if !defined(SWIG_PYTHON_THREAD_END_ALLOW) +# define SWIG_PYTHON_THREAD_END_ALLOW +# endif +# endif +#else /* No thread support */ +# define SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# define SWIG_PYTHON_THREAD_END_BLOCK +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# define SWIG_PYTHON_THREAD_END_ALLOW +#endif + +/* ----------------------------------------------------------------------------- + * Python API portion that goes into the runtime + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* ----------------------------------------------------------------------------- + * Constant declarations + * ----------------------------------------------------------------------------- */ + +/* Constant Types */ +#define SWIG_PY_POINTER 4 +#define SWIG_PY_BINARY 5 + +/* Constant information structure */ +typedef struct swig_const_info { + int type; + const char *name; + long lvalue; + double dvalue; + void *pvalue; + swig_type_info **ptype; +} swig_const_info; + +#ifdef __cplusplus +} +#endif + + +/* ----------------------------------------------------------------------------- + * pyrun.swg + * + * This file contains the runtime support for Python modules + * and includes code for managing global variables and pointer + * type checking. + * + * ----------------------------------------------------------------------------- */ + +#if PY_VERSION_HEX < 0x02070000 /* 2.7.0 */ +# error "This version of SWIG only supports Python >= 2.7" +#endif + +#if PY_VERSION_HEX >= 0x03000000 && PY_VERSION_HEX < 0x03020000 +# error "This version of SWIG only supports Python 3 >= 3.2" +#endif + +/* Common SWIG API */ + +/* for raw pointers */ +#define SWIG_Python_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, 0) +#define SWIG_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtr(obj, pptr, type, flags) +#define SWIG_ConvertPtrAndOwn(obj,pptr,type,flags,own) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, own) + +#ifdef SWIGPYTHON_BUILTIN +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(self, ptr, type, flags) +#else +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) +#endif + +#define SWIG_InternalNewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) + +#define SWIG_CheckImplicit(ty) SWIG_Python_CheckImplicit(ty) +#define SWIG_AcquirePtr(ptr, src) SWIG_Python_AcquirePtr(ptr, src) +#define swig_owntype int + +/* for raw packed data */ +#define SWIG_ConvertPacked(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewPackedObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + +/* for class or struct pointers */ +#define SWIG_ConvertInstance(obj, pptr, type, flags) SWIG_ConvertPtr(obj, pptr, type, flags) +#define SWIG_NewInstanceObj(ptr, type, flags) SWIG_NewPointerObj(ptr, type, flags) + +/* for C or C++ function pointers */ +#define SWIG_ConvertFunctionPtr(obj, pptr, type) SWIG_Python_ConvertFunctionPtr(obj, pptr, type) +#define SWIG_NewFunctionPtrObj(ptr, type) SWIG_Python_NewPointerObj(NULL, ptr, type, 0) + +/* for C++ member pointers, ie, member methods */ +#define SWIG_ConvertMember(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewMemberObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + + +/* Runtime API */ + +#define SWIG_GetModule(clientdata) SWIG_Python_GetModule(clientdata) +#define SWIG_SetModule(clientdata, pointer) SWIG_Python_SetModule(pointer) +#define SWIG_NewClientData(obj) SwigPyClientData_New(obj) + +#define SWIG_SetErrorObj SWIG_Python_SetErrorObj +#define SWIG_SetErrorMsg SWIG_Python_SetErrorMsg +#define SWIG_ErrorType(code) SWIG_Python_ErrorType(code) +#define SWIG_Error(code, msg) SWIG_Python_SetErrorMsg(SWIG_ErrorType(code), msg) +#define SWIG_fail goto fail + + +/* Runtime API implementation */ + +/* Error manipulation */ + +SWIGINTERN void +SWIG_Python_SetErrorObj(PyObject *errtype, PyObject *obj) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetObject(errtype, obj); + Py_DECREF(obj); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +SWIGINTERN void +SWIG_Python_SetErrorMsg(PyObject *errtype, const char *msg) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetString(errtype, msg); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +#define SWIG_Python_Raise(obj, type, desc) SWIG_Python_SetErrorObj(SWIG_Python_ExceptionType(desc), obj) + +/* Set a constant value */ + +#if defined(SWIGPYTHON_BUILTIN) + +SWIGINTERN void +SwigPyBuiltin_AddPublicSymbol(PyObject *seq, const char *key) { + PyObject *s = PyString_InternFromString(key); + PyList_Append(seq, s); + Py_DECREF(s); +} + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, PyObject *public_interface, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); + if (public_interface) + SwigPyBuiltin_AddPublicSymbol(public_interface, name); +} + +#else + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); +} + +#endif + +/* Append a value to the result obj */ + +SWIGINTERN PyObject* +SWIG_Python_AppendOutput(PyObject* result, PyObject* obj) { + if (!result) { + result = obj; + } else if (result == Py_None) { + Py_DECREF(result); + result = obj; + } else { + if (!PyList_Check(result)) { + PyObject *o2 = result; + result = PyList_New(1); + PyList_SetItem(result, 0, o2); + } + PyList_Append(result,obj); + Py_DECREF(obj); + } + return result; +} + +/* Unpack the argument tuple */ + +SWIGINTERN Py_ssize_t +SWIG_Python_UnpackTuple(PyObject *args, const char *name, Py_ssize_t min, Py_ssize_t max, PyObject **objs) +{ + if (!args) { + if (!min && !max) { + return 1; + } else { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got none", + name, (min == max ? "" : "at least "), (int)min); + return 0; + } + } + if (!PyTuple_Check(args)) { + if (min <= 1 && max >= 1) { + Py_ssize_t i; + objs[0] = args; + for (i = 1; i < max; ++i) { + objs[i] = 0; + } + return 2; + } + PyErr_SetString(PyExc_SystemError, "UnpackTuple() argument list is not a tuple"); + return 0; + } else { + Py_ssize_t l = PyTuple_GET_SIZE(args); + if (l < min) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at least "), (int)min, (int)l); + return 0; + } else if (l > max) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at most "), (int)max, (int)l); + return 0; + } else { + Py_ssize_t i; + for (i = 0; i < l; ++i) { + objs[i] = PyTuple_GET_ITEM(args, i); + } + for (; l < max; ++l) { + objs[l] = 0; + } + return i + 1; + } + } +} + +SWIGINTERN int +SWIG_Python_CheckNoKeywords(PyObject *kwargs, const char *name) { + int no_kwargs = 1; + if (kwargs) { + assert(PyDict_Check(kwargs)); + if (PyDict_Size(kwargs) > 0) { + PyErr_Format(PyExc_TypeError, "%s() does not take keyword arguments", name); + no_kwargs = 0; + } + } + return no_kwargs; +} + +/* A functor is a function object with one single object argument */ +#define SWIG_Python_CallFunctor(functor, obj) PyObject_CallFunctionObjArgs(functor, obj, NULL); + +/* + Helper for static pointer initialization for both C and C++ code, for example + static PyObject *SWIG_STATIC_POINTER(MyVar) = NewSomething(...); +*/ +#ifdef __cplusplus +#define SWIG_STATIC_POINTER(var) var +#else +#define SWIG_STATIC_POINTER(var) var = 0; if (!var) var +#endif + +/* ----------------------------------------------------------------------------- + * Pointer declarations + * ----------------------------------------------------------------------------- */ + +/* Flags for new pointer objects */ +#define SWIG_POINTER_NOSHADOW (SWIG_POINTER_OWN << 1) +#define SWIG_POINTER_NEW (SWIG_POINTER_NOSHADOW | SWIG_POINTER_OWN) + +#define SWIG_POINTER_IMPLICIT_CONV (SWIG_POINTER_DISOWN << 1) + +#define SWIG_BUILTIN_TP_INIT (SWIG_POINTER_OWN << 2) +#define SWIG_BUILTIN_INIT (SWIG_BUILTIN_TP_INIT | SWIG_POINTER_OWN) + +#ifdef __cplusplus +extern "C" { +#endif + +/* The python void return value */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Py_Void(void) +{ + PyObject *none = Py_None; + Py_INCREF(none); + return none; +} + +/* SwigPyClientData */ + +typedef struct { + PyObject *klass; + PyObject *newraw; + PyObject *newargs; + PyObject *destroy; + int delargs; + int implicitconv; + PyTypeObject *pytype; +} SwigPyClientData; + +SWIGRUNTIMEINLINE int +SWIG_Python_CheckImplicit(swig_type_info *ty) +{ + SwigPyClientData *data = (SwigPyClientData *)ty->clientdata; + int fail = data ? data->implicitconv : 0; + if (fail) + PyErr_SetString(PyExc_TypeError, "Implicit conversion is prohibited for explicit constructors."); + return fail; +} + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_ExceptionType(swig_type_info *desc) { + SwigPyClientData *data = desc ? (SwigPyClientData *) desc->clientdata : 0; + PyObject *klass = data ? data->klass : 0; + return (klass ? klass : PyExc_RuntimeError); +} + + +SWIGRUNTIME SwigPyClientData * +SwigPyClientData_New(PyObject* obj) +{ + if (!obj) { + return 0; + } else { + SwigPyClientData *data = (SwigPyClientData *)malloc(sizeof(SwigPyClientData)); + /* the klass element */ + data->klass = obj; + Py_INCREF(data->klass); + /* the newraw method and newargs arguments used to create a new raw instance */ + if (PyClass_Check(obj)) { + data->newraw = 0; + data->newargs = obj; + Py_INCREF(obj); + } else { + data->newraw = PyObject_GetAttrString(data->klass, "__new__"); + if (data->newraw) { + Py_INCREF(data->newraw); + data->newargs = PyTuple_New(1); + PyTuple_SetItem(data->newargs, 0, obj); + } else { + data->newargs = obj; + } + Py_INCREF(data->newargs); + } + /* the destroy method, aka as the C++ delete method */ + data->destroy = PyObject_GetAttrString(data->klass, "__swig_destroy__"); + if (PyErr_Occurred()) { + PyErr_Clear(); + data->destroy = 0; + } + if (data->destroy) { + int flags; + Py_INCREF(data->destroy); + flags = PyCFunction_GET_FLAGS(data->destroy); + data->delargs = !(flags & (METH_O)); + } else { + data->delargs = 0; + } + data->implicitconv = 0; + data->pytype = 0; + return data; + } +} + +SWIGRUNTIME void +SwigPyClientData_Del(SwigPyClientData *data) { + Py_XDECREF(data->newraw); + Py_XDECREF(data->newargs); + Py_XDECREF(data->destroy); +} + +/* =============== SwigPyObject =====================*/ + +typedef struct { + PyObject_HEAD + void *ptr; + swig_type_info *ty; + int own; + PyObject *next; +#ifdef SWIGPYTHON_BUILTIN + PyObject *dict; +#endif +} SwigPyObject; + + +#ifdef SWIGPYTHON_BUILTIN + +SWIGRUNTIME PyObject * +SwigPyObject_get___dict__(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + + if (!sobj->dict) + sobj->dict = PyDict_New(); + + Py_INCREF(sobj->dict); + return sobj->dict; +} + +#endif + +SWIGRUNTIME PyObject * +SwigPyObject_long(SwigPyObject *v) +{ + return PyLong_FromVoidPtr(v->ptr); +} + +SWIGRUNTIME PyObject * +SwigPyObject_format(const char* fmt, SwigPyObject *v) +{ + PyObject *res = NULL; + PyObject *args = PyTuple_New(1); + if (args) { + if (PyTuple_SetItem(args, 0, SwigPyObject_long(v)) == 0) { + PyObject *ofmt = SWIG_Python_str_FromChar(fmt); + if (ofmt) { +#if PY_VERSION_HEX >= 0x03000000 + res = PyUnicode_Format(ofmt,args); +#else + res = PyString_Format(ofmt,args); +#endif + Py_DECREF(ofmt); + } + Py_DECREF(args); + } + } + return res; +} + +SWIGRUNTIME PyObject * +SwigPyObject_oct(SwigPyObject *v) +{ + return SwigPyObject_format("%o",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_hex(SwigPyObject *v) +{ + return SwigPyObject_format("%x",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_repr(SwigPyObject *v) +{ + const char *name = SWIG_TypePrettyName(v->ty); + PyObject *repr = SWIG_Python_str_FromFormat("", (name ? name : "unknown"), (void *)v); + if (v->next) { + PyObject *nrep = SwigPyObject_repr((SwigPyObject *)v->next); +# if PY_VERSION_HEX >= 0x03000000 + PyObject *joined = PyUnicode_Concat(repr, nrep); + Py_DecRef(repr); + Py_DecRef(nrep); + repr = joined; +# else + PyString_ConcatAndDel(&repr,nrep); +# endif + } + return repr; +} + +/* We need a version taking two PyObject* parameters so it's a valid + * PyCFunction to use in swigobject_methods[]. */ +SWIGRUNTIME PyObject * +SwigPyObject_repr2(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + return SwigPyObject_repr((SwigPyObject*)v); +} + +SWIGRUNTIME int +SwigPyObject_compare(SwigPyObject *v, SwigPyObject *w) +{ + void *i = v->ptr; + void *j = w->ptr; + return (i < j) ? -1 : ((i > j) ? 1 : 0); +} + +/* Added for Python 3.x, would it also be useful for Python 2.x? */ +SWIGRUNTIME PyObject* +SwigPyObject_richcompare(SwigPyObject *v, SwigPyObject *w, int op) +{ + PyObject* res; + if( op != Py_EQ && op != Py_NE ) { + Py_INCREF(Py_NotImplemented); + return Py_NotImplemented; + } + res = PyBool_FromLong( (SwigPyObject_compare(v, w)==0) == (op == Py_EQ) ? 1 : 0); + return res; +} + + +SWIGRUNTIME PyTypeObject* SwigPyObject_TypeOnce(void); + +#ifdef SWIGPYTHON_BUILTIN +static swig_type_info *SwigPyObject_stype = 0; +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + SwigPyClientData *cd; + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + assert(cd); + assert(cd->pytype); + return cd->pytype; +} +#else +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyObject_TypeOnce(); + return type; +} +#endif + +SWIGRUNTIMEINLINE int +SwigPyObject_Check(PyObject *op) { +#ifdef SWIGPYTHON_BUILTIN + PyTypeObject *target_tp = SwigPyObject_type(); + if (PyType_IsSubtype(op->ob_type, target_tp)) + return 1; + return (strcmp(op->ob_type->tp_name, "SwigPyObject") == 0); +#else + return (Py_TYPE(op) == SwigPyObject_type()) + || (strcmp(Py_TYPE(op)->tp_name,"SwigPyObject") == 0); +#endif +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own); + +SWIGRUNTIME void +SwigPyObject_dealloc(PyObject *v) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + PyObject *next = sobj->next; + if (sobj->own == SWIG_POINTER_OWN) { + swig_type_info *ty = sobj->ty; + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + PyObject *destroy = data ? data->destroy : 0; + if (destroy) { + /* destroy is always a VARARGS method */ + PyObject *res; + + /* PyObject_CallFunction() has the potential to silently drop + the active exception. In cases of unnamed temporary + variable or where we just finished iterating over a generator + StopIteration will be active right now, and this needs to + remain true upon return from SwigPyObject_dealloc. So save + and restore. */ + + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); + + if (data->delargs) { + /* we need to create a temporary object to carry the destroy operation */ + PyObject *tmp = SwigPyObject_New(sobj->ptr, ty, 0); + res = SWIG_Python_CallFunctor(destroy, tmp); + Py_DECREF(tmp); + } else { + PyCFunction meth = PyCFunction_GET_FUNCTION(destroy); + PyObject *mself = PyCFunction_GET_SELF(destroy); + res = ((*meth)(mself, v)); + } + if (!res) + PyErr_WriteUnraisable(destroy); + + PyErr_Restore(type, value, traceback); + + Py_XDECREF(res); + } +#if !defined(SWIG_PYTHON_SILENT_MEMLEAK) + else { + const char *name = SWIG_TypePrettyName(ty); + printf("swig/python detected a memory leak of type '%s', no destructor found.\n", (name ? name : "unknown")); + } +#endif + } + Py_XDECREF(next); + PyObject_DEL(v); +} + +SWIGRUNTIME PyObject* +SwigPyObject_append(PyObject* v, PyObject* next) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (!SwigPyObject_Check(next)) { + PyErr_SetString(PyExc_TypeError, "Attempt to append a non SwigPyObject"); + return NULL; + } + sobj->next = next; + Py_INCREF(next); + return SWIG_Py_Void(); +} + +SWIGRUNTIME PyObject* +SwigPyObject_next(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (sobj->next) { + Py_INCREF(sobj->next); + return sobj->next; + } else { + return SWIG_Py_Void(); + } +} + +SWIGINTERN PyObject* +SwigPyObject_disown(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = 0; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_acquire(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = SWIG_POINTER_OWN; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_own(PyObject *v, PyObject *args) +{ + PyObject *val = 0; + if (!PyArg_UnpackTuple(args, "own", 0, 1, &val)) { + return NULL; + } else { + SwigPyObject *sobj = (SwigPyObject *)v; + PyObject *obj = PyBool_FromLong(sobj->own); + if (val) { + if (PyObject_IsTrue(val)) { + SwigPyObject_acquire(v,args); + } else { + SwigPyObject_disown(v,args); + } + } + return obj; + } +} + +static PyMethodDef +swigobject_methods[] = { + {"disown", SwigPyObject_disown, METH_NOARGS, "releases ownership of the pointer"}, + {"acquire", SwigPyObject_acquire, METH_NOARGS, "acquires ownership of the pointer"}, + {"own", SwigPyObject_own, METH_VARARGS, "returns/sets ownership of the pointer"}, + {"append", SwigPyObject_append, METH_O, "appends another 'this' object"}, + {"next", SwigPyObject_next, METH_NOARGS, "returns the next 'this' object"}, + {"__repr__",SwigPyObject_repr2, METH_NOARGS, "returns object representation"}, + {0, 0, 0, 0} +}; + +SWIGRUNTIME PyTypeObject* +SwigPyObject_TypeOnce(void) { + static char swigobject_doc[] = "Swig object carries a C/C++ instance pointer"; + + static PyNumberMethods SwigPyObject_as_number = { + (binaryfunc)0, /*nb_add*/ + (binaryfunc)0, /*nb_subtract*/ + (binaryfunc)0, /*nb_multiply*/ + /* nb_divide removed in Python 3 */ +#if PY_VERSION_HEX < 0x03000000 + (binaryfunc)0, /*nb_divide*/ +#endif + (binaryfunc)0, /*nb_remainder*/ + (binaryfunc)0, /*nb_divmod*/ + (ternaryfunc)0,/*nb_power*/ + (unaryfunc)0, /*nb_negative*/ + (unaryfunc)0, /*nb_positive*/ + (unaryfunc)0, /*nb_absolute*/ + (inquiry)0, /*nb_nonzero*/ + 0, /*nb_invert*/ + 0, /*nb_lshift*/ + 0, /*nb_rshift*/ + 0, /*nb_and*/ + 0, /*nb_xor*/ + 0, /*nb_or*/ +#if PY_VERSION_HEX < 0x03000000 + 0, /*nb_coerce*/ +#endif + (unaryfunc)SwigPyObject_long, /*nb_int*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_long, /*nb_long*/ +#else + 0, /*nb_reserved*/ +#endif + (unaryfunc)0, /*nb_float*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_oct, /*nb_oct*/ + (unaryfunc)SwigPyObject_hex, /*nb_hex*/ +#endif +#if PY_VERSION_HEX >= 0x03050000 /* 3.5 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_inplace_matrix_multiply */ +#elif PY_VERSION_HEX >= 0x03000000 /* 3.0 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index, nb_inplace_divide removed */ +#else + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index */ +#endif + }; + + static PyTypeObject swigpyobject_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyObject", /* tp_name */ + sizeof(SwigPyObject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyObject_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX >= 0x03000000 + 0, /* tp_reserved in 3.0.1, tp_compare in 3.0.0 but not used */ +#else + (cmpfunc)SwigPyObject_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyObject_repr, /* tp_repr */ + &SwigPyObject_as_number, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + 0, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigobject_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + (richcmpfunc)SwigPyObject_richcompare,/* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + swigobject_methods, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpyobject_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpyobject_type) < 0) + return NULL; + } + return &swigpyobject_type; +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own) +{ + SwigPyObject *sobj = PyObject_NEW(SwigPyObject, SwigPyObject_type()); + if (sobj) { + sobj->ptr = ptr; + sobj->ty = ty; + sobj->own = own; + sobj->next = 0; + } + return (PyObject *)sobj; +} + +/* ----------------------------------------------------------------------------- + * Implements a simple Swig Packed type, and use it instead of string + * ----------------------------------------------------------------------------- */ + +typedef struct { + PyObject_HEAD + void *pack; + swig_type_info *ty; + size_t size; +} SwigPyPacked; + +SWIGRUNTIME PyObject * +SwigPyPacked_repr(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))) { + return SWIG_Python_str_FromFormat("", result, v->ty->name); + } else { + return SWIG_Python_str_FromFormat("", v->ty->name); + } +} + +SWIGRUNTIME PyObject * +SwigPyPacked_str(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))){ + return SWIG_Python_str_FromFormat("%s%s", result, v->ty->name); + } else { + return SWIG_Python_str_FromChar(v->ty->name); + } +} + +SWIGRUNTIME int +SwigPyPacked_compare(SwigPyPacked *v, SwigPyPacked *w) +{ + size_t i = v->size; + size_t j = w->size; + int s = (i < j) ? -1 : ((i > j) ? 1 : 0); + return s ? s : strncmp((const char *)v->pack, (const char *)w->pack, 2*v->size); +} + +SWIGRUNTIME PyTypeObject* SwigPyPacked_TypeOnce(void); + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyPacked_TypeOnce(); + return type; +} + +SWIGRUNTIMEINLINE int +SwigPyPacked_Check(PyObject *op) { + return ((op)->ob_type == SwigPyPacked_TypeOnce()) + || (strcmp((op)->ob_type->tp_name,"SwigPyPacked") == 0); +} + +SWIGRUNTIME void +SwigPyPacked_dealloc(PyObject *v) +{ + if (SwigPyPacked_Check(v)) { + SwigPyPacked *sobj = (SwigPyPacked *) v; + free(sobj->pack); + } + PyObject_DEL(v); +} + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_TypeOnce(void) { + static char swigpacked_doc[] = "Swig object carries a C/C++ instance pointer"; + static PyTypeObject swigpypacked_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX>=0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyPacked", /* tp_name */ + sizeof(SwigPyPacked), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyPacked_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX>=0x03000000 + 0, /* tp_reserved in 3.0.1 */ +#else + (cmpfunc)SwigPyPacked_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyPacked_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + (reprfunc)SwigPyPacked_str, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigpacked_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + 0, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpypacked_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpypacked_type) < 0) + return NULL; + } + return &swigpypacked_type; +} + +SWIGRUNTIME PyObject * +SwigPyPacked_New(void *ptr, size_t size, swig_type_info *ty) +{ + SwigPyPacked *sobj = PyObject_NEW(SwigPyPacked, SwigPyPacked_type()); + if (sobj) { + void *pack = malloc(size); + if (pack) { + memcpy(pack, ptr, size); + sobj->pack = pack; + sobj->ty = ty; + sobj->size = size; + } else { + PyObject_DEL((PyObject *) sobj); + sobj = 0; + } + } + return (PyObject *) sobj; +} + +SWIGRUNTIME swig_type_info * +SwigPyPacked_UnpackData(PyObject *obj, void *ptr, size_t size) +{ + if (SwigPyPacked_Check(obj)) { + SwigPyPacked *sobj = (SwigPyPacked *)obj; + if (sobj->size != size) return 0; + memcpy(ptr, sobj->pack, size); + return sobj->ty; + } else { + return 0; + } +} + +/* ----------------------------------------------------------------------------- + * pointers/data manipulation + * ----------------------------------------------------------------------------- */ + +static PyObject *Swig_This_global = NULL; + +SWIGRUNTIME PyObject * +SWIG_This(void) +{ + if (Swig_This_global == NULL) + Swig_This_global = SWIG_Python_str_FromChar("this"); + return Swig_This_global; +} + +/* #define SWIG_PYTHON_SLOW_GETSET_THIS */ + +/* TODO: I don't know how to implement the fast getset in Python 3 right now */ +#if PY_VERSION_HEX>=0x03000000 +#define SWIG_PYTHON_SLOW_GETSET_THIS +#endif + +SWIGRUNTIME SwigPyObject * +SWIG_Python_GetSwigThis(PyObject *pyobj) +{ + PyObject *obj; + + if (SwigPyObject_Check(pyobj)) + return (SwigPyObject *) pyobj; + +#ifdef SWIGPYTHON_BUILTIN + (void)obj; +# ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + pyobj = PyWeakref_GET_OBJECT(pyobj); + if (pyobj && SwigPyObject_Check(pyobj)) + return (SwigPyObject*) pyobj; + } +# endif + return NULL; +#else + + obj = 0; + +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + if (PyInstance_Check(pyobj)) { + obj = _PyInstance_Lookup(pyobj, SWIG_This()); + } else { + PyObject **dictptr = _PyObject_GetDictPtr(pyobj); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + obj = dict ? PyDict_GetItem(dict, SWIG_This()) : 0; + } else { +#ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + PyObject *wobj = PyWeakref_GET_OBJECT(pyobj); + return wobj ? SWIG_Python_GetSwigThis(wobj) : 0; + } +#endif + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } + } + } +#else + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } +#endif + if (obj && !SwigPyObject_Check(obj)) { + /* a PyObject is called 'this', try to get the 'real this' + SwigPyObject from it */ + return SWIG_Python_GetSwigThis(obj); + } + return (SwigPyObject *)obj; +#endif +} + +/* Acquire a pointer value */ + +SWIGRUNTIME int +SWIG_Python_AcquirePtr(PyObject *obj, int own) { + if (own == SWIG_POINTER_OWN) { + SwigPyObject *sobj = SWIG_Python_GetSwigThis(obj); + if (sobj) { + int oldown = sobj->own; + sobj->own = own; + return oldown; + } + } + return 0; +} + +/* Convert a pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPtrAndOwn(PyObject *obj, void **ptr, swig_type_info *ty, int flags, int *own) { + int res; + SwigPyObject *sobj; + int implicit_conv = (flags & SWIG_POINTER_IMPLICIT_CONV) != 0; + + if (!obj) + return SWIG_ERROR; + if (obj == Py_None && !implicit_conv) { + if (ptr) + *ptr = 0; + return (flags & SWIG_POINTER_NO_NULL) ? SWIG_NullReferenceError : SWIG_OK; + } + + res = SWIG_ERROR; + + sobj = SWIG_Python_GetSwigThis(obj); + if (own) + *own = 0; + while (sobj) { + void *vptr = sobj->ptr; + if (ty) { + swig_type_info *to = sobj->ty; + if (to == ty) { + /* no type cast needed */ + if (ptr) *ptr = vptr; + break; + } else { + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) { + sobj = (SwigPyObject *)sobj->next; + } else { + if (ptr) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + if (newmemory == SWIG_CAST_NEW_MEMORY) { + assert(own); /* badly formed typemap which will lead to a memory leak - it must set and use own to delete *ptr */ + if (own) + *own = *own | SWIG_CAST_NEW_MEMORY; + } + } + break; + } + } + } else { + if (ptr) *ptr = vptr; + break; + } + } + if (sobj) { + if (own) + *own = *own | sobj->own; + if (flags & SWIG_POINTER_DISOWN) { + sobj->own = 0; + } + res = SWIG_OK; + } else { + if (implicit_conv) { + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + if (data && !data->implicitconv) { + PyObject *klass = data->klass; + if (klass) { + PyObject *impconv; + data->implicitconv = 1; /* avoid recursion and call 'explicit' constructors*/ + impconv = SWIG_Python_CallFunctor(klass, obj); + data->implicitconv = 0; + if (PyErr_Occurred()) { + PyErr_Clear(); + impconv = 0; + } + if (impconv) { + SwigPyObject *iobj = SWIG_Python_GetSwigThis(impconv); + if (iobj) { + void *vptr; + res = SWIG_Python_ConvertPtrAndOwn((PyObject*)iobj, &vptr, ty, 0, 0); + if (SWIG_IsOK(res)) { + if (ptr) { + *ptr = vptr; + /* transfer the ownership to 'ptr' */ + iobj->own = 0; + res = SWIG_AddCast(res); + res = SWIG_AddNewMask(res); + } else { + res = SWIG_AddCast(res); + } + } + } + Py_DECREF(impconv); + } + } + } + if (!SWIG_IsOK(res) && obj == Py_None) { + if (ptr) + *ptr = 0; + if (PyErr_Occurred()) + PyErr_Clear(); + res = SWIG_OK; + } + } + } + return res; +} + +/* Convert a function ptr value */ + +SWIGRUNTIME int +SWIG_Python_ConvertFunctionPtr(PyObject *obj, void **ptr, swig_type_info *ty) { + if (!PyCFunction_Check(obj)) { + return SWIG_ConvertPtr(obj, ptr, ty, 0); + } else { + void *vptr = 0; + swig_cast_info *tc; + + /* here we get the method pointer for callbacks */ + const char *doc = (((PyCFunctionObject *)obj) -> m_ml -> ml_doc); + const char *desc = doc ? strstr(doc, "swig_ptr: ") : 0; + if (desc) + desc = ty ? SWIG_UnpackVoidPtr(desc + 10, &vptr, ty->name) : 0; + if (!desc) + return SWIG_ERROR; + tc = SWIG_TypeCheck(desc,ty); + if (tc) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + assert(!newmemory); /* newmemory handling not yet implemented */ + } else { + return SWIG_ERROR; + } + return SWIG_OK; + } +} + +/* Convert a packed pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPacked(PyObject *obj, void *ptr, size_t sz, swig_type_info *ty) { + swig_type_info *to = SwigPyPacked_UnpackData(obj, ptr, sz); + if (!to) return SWIG_ERROR; + if (ty) { + if (to != ty) { + /* check type cast? */ + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) return SWIG_ERROR; + } + } + return SWIG_OK; +} + +/* ----------------------------------------------------------------------------- + * Create a new pointer object + * ----------------------------------------------------------------------------- */ + +/* + Create a new instance object, without calling __init__, and set the + 'this' attribute. +*/ + +SWIGRUNTIME PyObject* +SWIG_Python_NewShadowInstance(SwigPyClientData *data, PyObject *swig_this) +{ + PyObject *inst = 0; + PyObject *newraw = data->newraw; + if (newraw) { + inst = PyObject_Call(newraw, data->newargs, NULL); + if (inst) { +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + PyDict_SetItem(dict, SWIG_This(), swig_this); + } + } +#else + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } +#endif + } + } else { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *empty_args = PyTuple_New(0); + if (empty_args) { + PyObject *empty_kwargs = PyDict_New(); + if (empty_kwargs) { + inst = ((PyTypeObject *)data->newargs)->tp_new((PyTypeObject *)data->newargs, empty_args, empty_kwargs); + Py_DECREF(empty_kwargs); + if (inst) { + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } else { + Py_TYPE(inst)->tp_flags &= ~Py_TPFLAGS_VALID_VERSION_TAG; + } + } + } + Py_DECREF(empty_args); + } +#else + PyObject *dict = PyDict_New(); + if (dict) { + PyDict_SetItem(dict, SWIG_This(), swig_this); + inst = PyInstance_NewRaw(data->newargs, dict); + Py_DECREF(dict); + } +#endif + } + return inst; +} + +SWIGRUNTIME int +SWIG_Python_SetSwigThis(PyObject *inst, PyObject *swig_this) +{ +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + } + return PyDict_SetItem(dict, SWIG_This(), swig_this); + } +#endif + return PyObject_SetAttr(inst, SWIG_This(), swig_this); +} + + +SWIGINTERN PyObject * +SWIG_Python_InitShadowInstance(PyObject *args) { + PyObject *obj[2]; + if (!SWIG_Python_UnpackTuple(args, "swiginit", 2, 2, obj)) { + return NULL; + } else { + SwigPyObject *sthis = SWIG_Python_GetSwigThis(obj[0]); + if (sthis) { + SwigPyObject_append((PyObject*) sthis, obj[1]); + } else { + if (SWIG_Python_SetSwigThis(obj[0], obj[1]) != 0) + return NULL; + } + return SWIG_Py_Void(); + } +} + +/* Create a new pointer object */ + +SWIGRUNTIME PyObject * +SWIG_Python_NewPointerObj(PyObject *self, void *ptr, swig_type_info *type, int flags) { + SwigPyClientData *clientdata; + PyObject * robj; + int own; + + if (!ptr) + return SWIG_Py_Void(); + + clientdata = type ? (SwigPyClientData *)(type->clientdata) : 0; + own = (flags & SWIG_POINTER_OWN) ? SWIG_POINTER_OWN : 0; + if (clientdata && clientdata->pytype) { + SwigPyObject *newobj; + if (flags & SWIG_BUILTIN_TP_INIT) { + newobj = (SwigPyObject*) self; + if (newobj->ptr) { + PyObject *next_self = clientdata->pytype->tp_alloc(clientdata->pytype, 0); + while (newobj->next) + newobj = (SwigPyObject *) newobj->next; + newobj->next = next_self; + newobj = (SwigPyObject *)next_self; +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + } else { + newobj = PyObject_New(SwigPyObject, clientdata->pytype); +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + if (newobj) { + newobj->ptr = ptr; + newobj->ty = type; + newobj->own = own; + newobj->next = 0; + return (PyObject*) newobj; + } + return SWIG_Py_Void(); + } + + assert(!(flags & SWIG_BUILTIN_TP_INIT)); + + robj = SwigPyObject_New(ptr, type, own); + if (robj && clientdata && !(flags & SWIG_POINTER_NOSHADOW)) { + PyObject *inst = SWIG_Python_NewShadowInstance(clientdata, robj); + Py_DECREF(robj); + robj = inst; + } + return robj; +} + +/* Create a new packed object */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_NewPackedObj(void *ptr, size_t sz, swig_type_info *type) { + return ptr ? SwigPyPacked_New((void *) ptr, sz, type) : SWIG_Py_Void(); +} + +/* -----------------------------------------------------------------------------* + * Get type list + * -----------------------------------------------------------------------------*/ + +#ifdef SWIG_LINK_RUNTIME +void *SWIG_ReturnGlobalTypeList(void *); +#endif + +SWIGRUNTIME swig_module_info * +SWIG_Python_GetModule(void *SWIGUNUSEDPARM(clientdata)) { + static void *type_pointer = (void *)0; + /* first check if module already created */ + if (!type_pointer) { +#ifdef SWIG_LINK_RUNTIME + type_pointer = SWIG_ReturnGlobalTypeList((void *)0); +#else + type_pointer = PyCapsule_Import(SWIGPY_CAPSULE_NAME, 0); + if (PyErr_Occurred()) { + PyErr_Clear(); + type_pointer = (void *)0; + } +#endif + } + return (swig_module_info *) type_pointer; +} + +SWIGRUNTIME void +SWIG_Python_DestroyModule(PyObject *obj) +{ + swig_module_info *swig_module = (swig_module_info *) PyCapsule_GetPointer(obj, SWIGPY_CAPSULE_NAME); + swig_type_info **types = swig_module->types; + size_t i; + for (i =0; i < swig_module->size; ++i) { + swig_type_info *ty = types[i]; + if (ty->owndata) { + SwigPyClientData *data = (SwigPyClientData *) ty->clientdata; + if (data) SwigPyClientData_Del(data); + } + } + Py_DECREF(SWIG_This()); + Swig_This_global = NULL; +} + +SWIGRUNTIME void +SWIG_Python_SetModule(swig_module_info *swig_module) { +#if PY_VERSION_HEX >= 0x03000000 + /* Add a dummy module object into sys.modules */ + PyObject *module = PyImport_AddModule("swig_runtime_data" SWIG_RUNTIME_VERSION); +#else + static PyMethodDef swig_empty_runtime_method_table[] = { {NULL, NULL, 0, NULL} }; /* Sentinel */ + PyObject *module = Py_InitModule("swig_runtime_data" SWIG_RUNTIME_VERSION, swig_empty_runtime_method_table); +#endif + PyObject *pointer = PyCapsule_New((void *) swig_module, SWIGPY_CAPSULE_NAME, SWIG_Python_DestroyModule); + if (pointer && module) { + PyModule_AddObject(module, "type_pointer_capsule" SWIG_TYPE_TABLE_NAME, pointer); + } else { + Py_XDECREF(pointer); + } +} + +/* The python cached type query */ +SWIGRUNTIME PyObject * +SWIG_Python_TypeCache(void) { + static PyObject *SWIG_STATIC_POINTER(cache) = PyDict_New(); + return cache; +} + +SWIGRUNTIME swig_type_info * +SWIG_Python_TypeQuery(const char *type) +{ + PyObject *cache = SWIG_Python_TypeCache(); + PyObject *key = SWIG_Python_str_FromChar(type); + PyObject *obj = PyDict_GetItem(cache, key); + swig_type_info *descriptor; + if (obj) { + descriptor = (swig_type_info *) PyCapsule_GetPointer(obj, NULL); + } else { + swig_module_info *swig_module = SWIG_GetModule(0); + descriptor = SWIG_TypeQueryModule(swig_module, swig_module, type); + if (descriptor) { + obj = PyCapsule_New((void*) descriptor, NULL, NULL); + PyDict_SetItem(cache, key, obj); + Py_DECREF(obj); + } + } + Py_DECREF(key); + return descriptor; +} + +/* + For backward compatibility only +*/ +#define SWIG_POINTER_EXCEPTION 0 +#define SWIG_arg_fail(arg) SWIG_Python_ArgFail(arg) +#define SWIG_MustGetPtr(p, type, argnum, flags) SWIG_Python_MustGetPtr(p, type, argnum, flags) + +SWIGRUNTIME int +SWIG_Python_AddErrMesg(const char* mesg, int infront) +{ + if (PyErr_Occurred()) { + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + const char *errmesg = tmp ? tmp : "Invalid error message"; + Py_XINCREF(type); + PyErr_Clear(); + if (infront) { + PyErr_Format(type, "%s %s", mesg, errmesg); + } else { + PyErr_Format(type, "%s %s", errmesg, mesg); + } + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + } + return 1; + } else { + return 0; + } +} + +SWIGRUNTIME int +SWIG_Python_ArgFail(int argnum) +{ + if (PyErr_Occurred()) { + /* add information about failing argument */ + char mesg[256]; + PyOS_snprintf(mesg, sizeof(mesg), "argument number %d:", argnum); + return SWIG_Python_AddErrMesg(mesg, 1); + } else { + return 0; + } +} + +SWIGRUNTIMEINLINE const char * +SwigPyObject_GetDesc(PyObject *self) +{ + SwigPyObject *v = (SwigPyObject *)self; + swig_type_info *ty = v ? v->ty : 0; + return ty ? ty->str : ""; +} + +SWIGRUNTIME void +SWIG_Python_TypeError(const char *type, PyObject *obj) +{ + if (type) { +#if defined(SWIG_COBJECT_TYPES) + if (obj && SwigPyObject_Check(obj)) { + const char *otype = (const char *) SwigPyObject_GetDesc(obj); + if (otype) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, 'SwigPyObject(%s)' is received", + type, otype); + return; + } + } else +#endif + { + const char *otype = (obj ? obj->ob_type->tp_name : 0); + if (otype) { + PyObject *str = PyObject_Str(obj); + const char *cstr = str ? SWIG_Python_str_AsChar(str) : 0; + if (cstr) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s(%s)' is received", + type, otype, cstr); + SWIG_Python_str_DelForPy3(cstr); + } else { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s' is received", + type, otype); + } + Py_XDECREF(str); + return; + } + } + PyErr_Format(PyExc_TypeError, "a '%s' is expected", type); + } else { + PyErr_Format(PyExc_TypeError, "unexpected type is received"); + } +} + + +/* Convert a pointer value, signal an exception on a type mismatch */ +SWIGRUNTIME void * +SWIG_Python_MustGetPtr(PyObject *obj, swig_type_info *ty, int SWIGUNUSEDPARM(argnum), int flags) { + void *result; + if (SWIG_Python_ConvertPtr(obj, &result, ty, flags) == -1) { + PyErr_Clear(); +#if SWIG_POINTER_EXCEPTION + if (flags) { + SWIG_Python_TypeError(SWIG_TypePrettyName(ty), obj); + SWIG_Python_ArgFail(argnum); + } +#endif + } + return result; +} + +#ifdef SWIGPYTHON_BUILTIN +SWIGRUNTIME int +SWIG_Python_NonDynamicSetAttr(PyObject *obj, PyObject *name, PyObject *value) { + PyTypeObject *tp = obj->ob_type; + PyObject *descr; + PyObject *encoded_name; + descrsetfunc f; + int res = -1; + +# ifdef Py_USING_UNICODE + if (PyString_Check(name)) { + name = PyUnicode_Decode(PyString_AsString(name), PyString_Size(name), NULL, NULL); + if (!name) + return -1; + } else if (!PyUnicode_Check(name)) +# else + if (!PyString_Check(name)) +# endif + { + PyErr_Format(PyExc_TypeError, "attribute name must be string, not '%.200s'", name->ob_type->tp_name); + return -1; + } else { + Py_INCREF(name); + } + + if (!tp->tp_dict) { + if (PyType_Ready(tp) < 0) + goto done; + } + + descr = _PyType_Lookup(tp, name); + f = NULL; + if (descr != NULL) + f = descr->ob_type->tp_descr_set; + if (!f) { + if (PyString_Check(name)) { + encoded_name = name; + Py_INCREF(name); + } else { + encoded_name = PyUnicode_AsUTF8String(name); + if (!encoded_name) + return -1; + } + PyErr_Format(PyExc_AttributeError, "'%.100s' object has no attribute '%.200s'", tp->tp_name, PyString_AsString(encoded_name)); + Py_DECREF(encoded_name); + } else { + res = f(descr, obj, value); + } + + done: + Py_DECREF(name); + return res; +} +#endif + + +#ifdef __cplusplus +} +#endif + + + +#define SWIG_exception_fail(code, msg) do { SWIG_Error(code, msg); SWIG_fail; } while(0) + +#define SWIG_contract_assert(expr, msg) if (!(expr)) { SWIG_Error(SWIG_RuntimeError, msg); SWIG_fail; } else + + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Method creation and docstring support functions */ + +SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name); +SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); +SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); + +#ifdef __cplusplus +} +#endif + + +/* -------- TYPES TABLE (BEGIN) -------- */ + +#define SWIGTYPE_p_ControlMode swig_types[0] +#define SWIGTYPE_p_CoordinateSystem swig_types[1] +#define SWIGTYPE_p_EngineType swig_types[2] +#define SWIGTYPE_p_IndicatorState swig_types[3] +#define SWIGTYPE_p_Mode swig_types[4] +#define SWIGTYPE_p_SimulationMode swig_types[5] +#define SWIGTYPE_p_Type swig_types[6] +#define SWIGTYPE_p_UserInputEvent swig_types[7] +#define SWIGTYPE_p_WbCameraRecognitionObject swig_types[8] +#define SWIGTYPE_p_WbContactPoint swig_types[9] +#define SWIGTYPE_p_WbLidarPoint swig_types[10] +#define SWIGTYPE_p_WbMouseState swig_types[11] +#define SWIGTYPE_p_WbRadarTarget swig_types[12] +#define SWIGTYPE_p_WheelIndex swig_types[13] +#define SWIGTYPE_p_WiperMode swig_types[14] +#define SWIGTYPE_p_char swig_types[15] +#define SWIGTYPE_p_webots__Car swig_types[16] +#define SWIGTYPE_p_webots__Driver swig_types[17] +#define SWIGTYPE_p_webots__Robot swig_types[18] +#define SWIGTYPE_p_webots__Supervisor swig_types[19] +static swig_type_info *swig_types[21]; +static swig_module_info swig_module = {swig_types, 20, 0, 0, 0, 0}; +#define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name) +#define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name) + +/* -------- TYPES TABLE (END) -------- */ + +#ifdef SWIG_TypeQuery +# undef SWIG_TypeQuery +#endif +#define SWIG_TypeQuery SWIG_Python_TypeQuery + +/*----------------------------------------------- + @(target):= _vehicle.so + ------------------------------------------------*/ +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_init PyInit__vehicle + +#else +# define SWIG_init init_vehicle + +#endif +#define SWIG_name "_vehicle" + +#define SWIGVERSION 0x040002 +#define SWIG_VERSION SWIGVERSION + + +#define SWIG_as_voidptr(a) const_cast< void * >(static_cast< const void * >(a)) +#define SWIG_as_voidptrptr(a) ((void)SWIG_as_voidptr(*a),reinterpret_cast< void** >(a)) + + +#include + + +namespace swig { + class SwigPtr_PyObject { + protected: + PyObject *_obj; + + public: + SwigPtr_PyObject() :_obj(0) + { + } + + SwigPtr_PyObject(const SwigPtr_PyObject& item) : _obj(item._obj) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + SwigPtr_PyObject(PyObject *obj, bool initial_ref = true) :_obj(obj) + { + if (initial_ref) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + } + + SwigPtr_PyObject & operator=(const SwigPtr_PyObject& item) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(item._obj); + Py_XDECREF(_obj); + _obj = item._obj; + SWIG_PYTHON_THREAD_END_BLOCK; + return *this; + } + + ~SwigPtr_PyObject() + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XDECREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + operator PyObject *() const + { + return _obj; + } + + PyObject *operator->() const + { + return _obj; + } + }; +} + + +namespace swig { + struct SwigVar_PyObject : SwigPtr_PyObject { + SwigVar_PyObject(PyObject* obj = 0) : SwigPtr_PyObject(obj, false) { } + + SwigVar_PyObject & operator = (PyObject* obj) + { + Py_XDECREF(_obj); + _obj = obj; + return *this; + } + }; +} + + +#include +#include + + +SWIGINTERNINLINE PyObject* + SWIG_From_int (int value) +{ + return PyInt_FromLong((long) value); +} + + +SWIGINTERNINLINE PyObject* + SWIG_From_bool (bool value) +{ + return PyBool_FromLong(value ? 1 : 0); +} + + +SWIGINTERN int +SWIG_AsVal_double (PyObject *obj, double *val) +{ + int res = SWIG_TypeError; + if (PyFloat_Check(obj)) { + if (val) *val = PyFloat_AsDouble(obj); + return SWIG_OK; +#if PY_VERSION_HEX < 0x03000000 + } else if (PyInt_Check(obj)) { + if (val) *val = (double) PyInt_AsLong(obj); + return SWIG_OK; +#endif + } else if (PyLong_Check(obj)) { + double v = PyLong_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + double d = PyFloat_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = d; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_AddCast(SWIG_OK)); + } else { + PyErr_Clear(); + } + } + } +#endif + return res; +} + + + #define SWIG_From_double PyFloat_FromDouble + + +#include +#if !defined(SWIG_NO_LLONG_MAX) +# if !defined(LLONG_MAX) && defined(__GNUC__) && defined (__LONG_LONG_MAX__) +# define LLONG_MAX __LONG_LONG_MAX__ +# define LLONG_MIN (-LLONG_MAX - 1LL) +# define ULLONG_MAX (LLONG_MAX * 2ULL + 1ULL) +# endif +#endif + + +#include + + +#include + + +SWIGINTERNINLINE int +SWIG_CanCastAsInteger(double *d, double min, double max) { + double x = *d; + if ((min <= x && x <= max)) { + double fx = floor(x); + double cx = ceil(x); + double rd = ((x - fx) < 0.5) ? fx : cx; /* simple rint */ + if ((errno == EDOM) || (errno == ERANGE)) { + errno = 0; + } else { + double summ, reps, diff; + if (rd < x) { + diff = x - rd; + } else if (rd > x) { + diff = rd - x; + } else { + return 1; + } + summ = rd + x; + reps = diff/summ; + if (reps < 8*DBL_EPSILON) { + *d = rd; + return 1; + } + } + } + return 0; +} + + +SWIGINTERN int +SWIG_AsVal_long (PyObject *obj, long* val) +{ +#if PY_VERSION_HEX < 0x03000000 + if (PyInt_Check(obj)) { + if (val) *val = PyInt_AsLong(obj); + return SWIG_OK; + } else +#endif + if (PyLong_Check(obj)) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + return SWIG_OverflowError; + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + long v = PyInt_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + double d; + int res = SWIG_AddCast(SWIG_AsVal_double (obj,&d)); + if (SWIG_IsOK(res) && SWIG_CanCastAsInteger(&d, LONG_MIN, LONG_MAX)) { + if (val) *val = (long)(d); + return res; + } + } + } +#endif + return SWIG_TypeError; +} + + +SWIGINTERN int +SWIG_AsVal_int (PyObject * obj, int *val) +{ + long v; + int res = SWIG_AsVal_long (obj, &v); + if (SWIG_IsOK(res)) { + if ((v < INT_MIN || v > INT_MAX)) { + return SWIG_OverflowError; + } else { + if (val) *val = static_cast< int >(v); + } + } + return res; +} + + +SWIGINTERN int +SWIG_AsVal_bool (PyObject *obj, bool *val) +{ + int r; + if (!PyBool_Check(obj)) + return SWIG_ERROR; + r = PyObject_IsTrue(obj); + if (r == -1) + return SWIG_ERROR; + if (val) *val = r ? true : false; + return SWIG_OK; +} + +#ifdef __cplusplus +extern "C" { +#endif +SWIGINTERN PyObject *_wrap_new_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Driver", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)new webots::Driver(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDriverInstance(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_getDriverInstance", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)webots::Driver::getDriverInstance(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, 0 | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Driver" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_isInitialisationPossible(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + bool result; + + if (!SWIG_Python_UnpackTuple(args, "Driver_isInitialisationPossible", 0, 0, 0)) SWIG_fail; + result = (bool)webots::Driver::isInitialisationPossible(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_step(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_step" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->step(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setCruisingSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setCruisingSpeed" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setCruisingSpeed(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getTargetCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getTargetCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getTargetCruisingSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getCurrentSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getCurrentSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getCurrentSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setThrottle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setThrottle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setThrottle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getThrottle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrakeIntensity", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrakeIntensity" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrakeIntensity(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getBrakeIntensity(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::IndicatorState arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setIndicator", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setIndicator" "', argument " "2"" of type '" "webots::Driver::IndicatorState""'"); + } + arg2 = static_cast< webots::Driver::IndicatorState >(val2); + (arg1)->setIndicator(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setHazardFlashers", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setHazardFlashers" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setHazardFlashers(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::IndicatorState result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::IndicatorState)(arg1)->getIndicator(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getHazardFlashers(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setDippedBeams", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setDippedBeams" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setDippedBeams(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setAntifogLights", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setAntifogLights" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setAntifogLights(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getDippedBeams(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getAntifogLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getRpm(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getRpm" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getRpm(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGear(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + int arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setGear", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setGear" "', argument " "2"" of type '" "int""'"); + } + arg2 = static_cast< int >(val2); + (arg1)->setGear(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGearNumber(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGearNumber" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGearNumber(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getControlMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::ControlMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getControlMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::ControlMode)(arg1)->getControlMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWiperMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWiperMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWiperMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWiperMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrake(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrake", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrake" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrake" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrake(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWipersMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWipersMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWipersMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWipersMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Driver_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Driver, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Driver_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +SWIGINTERN PyObject *_wrap_new_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Car", 0, 0, 0)) SWIG_fail; + result = (webots::Car *)new webots::Car(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Car, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Car" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::Type result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::Type)(arg1)->getType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getEngineType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::EngineType result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getEngineType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::EngineType)(arg1)->getEngineType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setIndicatorPeriod", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setIndicatorPeriod" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setIndicatorPeriod(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getIndicatorPeriod(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBackwardsLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBackwardsLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBackwardsLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBrakeLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBrakeLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBrakeLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackFront(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackFront" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackFront(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackRear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackRear" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackRear(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelbase(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelbase" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getWheelbase(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getFrontWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getFrontWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getFrontWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRearWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRearWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRearWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelEncoder(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelEncoder", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelEncoder" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelEncoder" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelEncoder(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelSpeed" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelSpeed" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelSpeed(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setLeftSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setLeftSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setLeftSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setRightSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setRightSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setRightSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRightSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getLeftSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableLimitedSlipDifferential(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableLimitedSlipDifferential", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableLimitedSlipDifferential(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableIndicatorAutoDisabling(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableIndicatorAutoDisabling", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableIndicatorAutoDisabling(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Car_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Car, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Car_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +static PyMethodDef SwigMethods[] = { + { "SWIG_PyInstanceMethod_New", SWIG_PyInstanceMethod_New, METH_O, NULL}, + { "new_Driver", _wrap_new_Driver, METH_NOARGS, NULL}, + { "Driver_getDriverInstance", _wrap_Driver_getDriverInstance, METH_NOARGS, NULL}, + { "delete_Driver", _wrap_delete_Driver, METH_O, NULL}, + { "Driver_isInitialisationPossible", _wrap_Driver_isInitialisationPossible, METH_NOARGS, NULL}, + { "Driver_step", _wrap_Driver_step, METH_O, NULL}, + { "Driver_setSteeringAngle", _wrap_Driver_setSteeringAngle, METH_VARARGS, NULL}, + { "Driver_getSteeringAngle", _wrap_Driver_getSteeringAngle, METH_O, NULL}, + { "Driver_setCruisingSpeed", _wrap_Driver_setCruisingSpeed, METH_VARARGS, NULL}, + { "Driver_getTargetCruisingSpeed", _wrap_Driver_getTargetCruisingSpeed, METH_O, NULL}, + { "Driver_getCurrentSpeed", _wrap_Driver_getCurrentSpeed, METH_O, NULL}, + { "Driver_setThrottle", _wrap_Driver_setThrottle, METH_VARARGS, NULL}, + { "Driver_getThrottle", _wrap_Driver_getThrottle, METH_O, NULL}, + { "Driver_setBrakeIntensity", _wrap_Driver_setBrakeIntensity, METH_VARARGS, NULL}, + { "Driver_getBrakeIntensity", _wrap_Driver_getBrakeIntensity, METH_O, NULL}, + { "Driver_setIndicator", _wrap_Driver_setIndicator, METH_VARARGS, NULL}, + { "Driver_setHazardFlashers", _wrap_Driver_setHazardFlashers, METH_VARARGS, NULL}, + { "Driver_getIndicator", _wrap_Driver_getIndicator, METH_O, NULL}, + { "Driver_getHazardFlashers", _wrap_Driver_getHazardFlashers, METH_O, NULL}, + { "Driver_setDippedBeams", _wrap_Driver_setDippedBeams, METH_VARARGS, NULL}, + { "Driver_setAntifogLights", _wrap_Driver_setAntifogLights, METH_VARARGS, NULL}, + { "Driver_getDippedBeams", _wrap_Driver_getDippedBeams, METH_O, NULL}, + { "Driver_getAntifogLights", _wrap_Driver_getAntifogLights, METH_O, NULL}, + { "Driver_getRpm", _wrap_Driver_getRpm, METH_O, NULL}, + { "Driver_getGear", _wrap_Driver_getGear, METH_O, NULL}, + { "Driver_setGear", _wrap_Driver_setGear, METH_VARARGS, NULL}, + { "Driver_getGearNumber", _wrap_Driver_getGearNumber, METH_O, NULL}, + { "Driver_getControlMode", _wrap_Driver_getControlMode, METH_O, NULL}, + { "Driver_setWiperMode", _wrap_Driver_setWiperMode, METH_VARARGS, NULL}, + { "Driver_getWiperMode", _wrap_Driver_getWiperMode, METH_O, NULL}, + { "Driver_setBrake", _wrap_Driver_setBrake, METH_VARARGS, NULL}, + { "Driver_setWipersMode", _wrap_Driver_setWipersMode, METH_VARARGS, NULL}, + { "Driver_getWipersMode", _wrap_Driver_getWipersMode, METH_O, NULL}, + { "Driver_swigregister", Driver_swigregister, METH_O, NULL}, + { "Driver_swiginit", Driver_swiginit, METH_VARARGS, NULL}, + { "new_Car", _wrap_new_Car, METH_NOARGS, NULL}, + { "delete_Car", _wrap_delete_Car, METH_O, NULL}, + { "Car_getType", _wrap_Car_getType, METH_O, NULL}, + { "Car_getEngineType", _wrap_Car_getEngineType, METH_O, NULL}, + { "Car_setIndicatorPeriod", _wrap_Car_setIndicatorPeriod, METH_VARARGS, NULL}, + { "Car_getIndicatorPeriod", _wrap_Car_getIndicatorPeriod, METH_O, NULL}, + { "Car_getBackwardsLights", _wrap_Car_getBackwardsLights, METH_O, NULL}, + { "Car_getBrakeLights", _wrap_Car_getBrakeLights, METH_O, NULL}, + { "Car_getTrackFront", _wrap_Car_getTrackFront, METH_O, NULL}, + { "Car_getTrackRear", _wrap_Car_getTrackRear, METH_O, NULL}, + { "Car_getWheelbase", _wrap_Car_getWheelbase, METH_O, NULL}, + { "Car_getFrontWheelRadius", _wrap_Car_getFrontWheelRadius, METH_O, NULL}, + { "Car_getRearWheelRadius", _wrap_Car_getRearWheelRadius, METH_O, NULL}, + { "Car_getWheelEncoder", _wrap_Car_getWheelEncoder, METH_VARARGS, NULL}, + { "Car_getWheelSpeed", _wrap_Car_getWheelSpeed, METH_VARARGS, NULL}, + { "Car_setLeftSteeringAngle", _wrap_Car_setLeftSteeringAngle, METH_VARARGS, NULL}, + { "Car_setRightSteeringAngle", _wrap_Car_setRightSteeringAngle, METH_VARARGS, NULL}, + { "Car_getRightSteeringAngle", _wrap_Car_getRightSteeringAngle, METH_O, NULL}, + { "Car_getLeftSteeringAngle", _wrap_Car_getLeftSteeringAngle, METH_O, NULL}, + { "Car_enableLimitedSlipDifferential", _wrap_Car_enableLimitedSlipDifferential, METH_VARARGS, NULL}, + { "Car_enableIndicatorAutoDisabling", _wrap_Car_enableIndicatorAutoDisabling, METH_VARARGS, NULL}, + { "Car_swigregister", Car_swigregister, METH_O, NULL}, + { "Car_swiginit", Car_swiginit, METH_VARARGS, NULL}, + { NULL, NULL, 0, NULL } +}; + +static PyMethodDef SwigMethods_proxydocs[] = { + { NULL, NULL, 0, NULL } +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (BEGIN) -------- */ + +static void *_p_webots__DriverTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__CarTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) (webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__DriverTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__SupervisorTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) ((webots::Supervisor *) x)); +} +static void *_p_webots__CarTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *)(webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__CarTo_p_webots__Driver(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Driver *) ((webots::Car *) x)); +} +static swig_type_info _swigt__p_ControlMode = {"_p_ControlMode", "ControlMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_CoordinateSystem = {"_p_CoordinateSystem", "CoordinateSystem *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_EngineType = {"_p_EngineType", "EngineType *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_IndicatorState = {"_p_IndicatorState", "IndicatorState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Mode = {"_p_Mode", "Mode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_SimulationMode = {"_p_SimulationMode", "SimulationMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Type = {"_p_Type", "Type *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_UserInputEvent = {"_p_UserInputEvent", "UserInputEvent *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbCameraRecognitionObject = {"_p_WbCameraRecognitionObject", "WbCameraRecognitionObject *|webots::CameraRecognitionObject *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbContactPoint = {"_p_WbContactPoint", "WbContactPoint *|webots::ContactPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbLidarPoint = {"_p_WbLidarPoint", "WbLidarPoint *|webots::LidarPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbMouseState = {"_p_WbMouseState", "WbMouseState *|webots::MouseState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbRadarTarget = {"_p_WbRadarTarget", "WbRadarTarget *|webots::RadarTarget *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WheelIndex = {"_p_WheelIndex", "WheelIndex *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WiperMode = {"_p_WiperMode", "WiperMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_char = {"_p_char", "char *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Car = {"_p_webots__Car", "webots::Car *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Driver = {"_p_webots__Driver", "webots::Driver *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Robot = {"_p_webots__Robot", "webots::Robot *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Supervisor = {"_p_webots__Supervisor", "webots::Supervisor *", 0, 0, (void*)0, 0}; + +static swig_type_info *swig_type_initial[] = { + &_swigt__p_ControlMode, + &_swigt__p_CoordinateSystem, + &_swigt__p_EngineType, + &_swigt__p_IndicatorState, + &_swigt__p_Mode, + &_swigt__p_SimulationMode, + &_swigt__p_Type, + &_swigt__p_UserInputEvent, + &_swigt__p_WbCameraRecognitionObject, + &_swigt__p_WbContactPoint, + &_swigt__p_WbLidarPoint, + &_swigt__p_WbMouseState, + &_swigt__p_WbRadarTarget, + &_swigt__p_WheelIndex, + &_swigt__p_WiperMode, + &_swigt__p_char, + &_swigt__p_webots__Car, + &_swigt__p_webots__Driver, + &_swigt__p_webots__Robot, + &_swigt__p_webots__Supervisor, +}; + +static swig_cast_info _swigc__p_ControlMode[] = { {&_swigt__p_ControlMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_CoordinateSystem[] = { {&_swigt__p_CoordinateSystem, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_EngineType[] = { {&_swigt__p_EngineType, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_IndicatorState[] = { {&_swigt__p_IndicatorState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Mode[] = { {&_swigt__p_Mode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_SimulationMode[] = { {&_swigt__p_SimulationMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Type[] = { {&_swigt__p_Type, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_UserInputEvent[] = { {&_swigt__p_UserInputEvent, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbCameraRecognitionObject[] = { {&_swigt__p_WbCameraRecognitionObject, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbContactPoint[] = { {&_swigt__p_WbContactPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbLidarPoint[] = { {&_swigt__p_WbLidarPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbMouseState[] = { {&_swigt__p_WbMouseState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbRadarTarget[] = { {&_swigt__p_WbRadarTarget, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WheelIndex[] = { {&_swigt__p_WheelIndex, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WiperMode[] = { {&_swigt__p_WiperMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_char[] = { {&_swigt__p_char, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Car[] = { {&_swigt__p_webots__Car, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Driver[] = { {&_swigt__p_webots__Driver, 0, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Driver, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Robot[] = { {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Supervisor, _p_webots__SupervisorTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Robot, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Supervisor[] = { {&_swigt__p_webots__Supervisor, 0, 0, 0}, {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Supervisor, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Supervisor, 0, 0},{0, 0, 0, 0}}; + +static swig_cast_info *swig_cast_initial[] = { + _swigc__p_ControlMode, + _swigc__p_CoordinateSystem, + _swigc__p_EngineType, + _swigc__p_IndicatorState, + _swigc__p_Mode, + _swigc__p_SimulationMode, + _swigc__p_Type, + _swigc__p_UserInputEvent, + _swigc__p_WbCameraRecognitionObject, + _swigc__p_WbContactPoint, + _swigc__p_WbLidarPoint, + _swigc__p_WbMouseState, + _swigc__p_WbRadarTarget, + _swigc__p_WheelIndex, + _swigc__p_WiperMode, + _swigc__p_char, + _swigc__p_webots__Car, + _swigc__p_webots__Driver, + _swigc__p_webots__Robot, + _swigc__p_webots__Supervisor, +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (END) -------- */ + +static swig_const_info swig_const_table[] = { +{0, 0, 0, 0.0, 0, 0}}; + +#ifdef __cplusplus +} +#endif +/* ----------------------------------------------------------------------------- + * Type initialization: + * This problem is tough by the requirement that no dynamic + * memory is used. Also, since swig_type_info structures store pointers to + * swig_cast_info structures and swig_cast_info structures store pointers back + * to swig_type_info structures, we need some lookup code at initialization. + * The idea is that swig generates all the structures that are needed. + * The runtime then collects these partially filled structures. + * The SWIG_InitializeModule function takes these initial arrays out of + * swig_module, and does all the lookup, filling in the swig_module.types + * array with the correct data and linking the correct swig_cast_info + * structures together. + * + * The generated swig_type_info structures are assigned statically to an initial + * array. We just loop through that array, and handle each type individually. + * First we lookup if this type has been already loaded, and if so, use the + * loaded structure instead of the generated one. Then we have to fill in the + * cast linked list. The cast data is initially stored in something like a + * two-dimensional array. Each row corresponds to a type (there are the same + * number of rows as there are in the swig_type_initial array). Each entry in + * a column is one of the swig_cast_info structures for that type. + * The cast_initial array is actually an array of arrays, because each row has + * a variable number of columns. So to actually build the cast linked list, + * we find the array of casts associated with the type, and loop through it + * adding the casts to the list. The one last trick we need to do is making + * sure the type pointer in the swig_cast_info struct is correct. + * + * First off, we lookup the cast->type name to see if it is already loaded. + * There are three cases to handle: + * 1) If the cast->type has already been loaded AND the type we are adding + * casting info to has not been loaded (it is in this module), THEN we + * replace the cast->type pointer with the type pointer that has already + * been loaded. + * 2) If BOTH types (the one we are adding casting info to, and the + * cast->type) are loaded, THEN the cast info has already been loaded by + * the previous module so we just ignore it. + * 3) Finally, if cast->type has not already been loaded, then we add that + * swig_cast_info to the linked list (because the cast->type) pointer will + * be correct. + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#if 0 +} /* c-mode */ +#endif +#endif + +#if 0 +#define SWIGRUNTIME_DEBUG +#endif + + +SWIGRUNTIME void +SWIG_InitializeModule(void *clientdata) { + size_t i; + swig_module_info *module_head, *iter; + int init; + + /* check to see if the circular list has been setup, if not, set it up */ + if (swig_module.next==0) { + /* Initialize the swig_module */ + swig_module.type_initial = swig_type_initial; + swig_module.cast_initial = swig_cast_initial; + swig_module.next = &swig_module; + init = 1; + } else { + init = 0; + } + + /* Try and load any already created modules */ + module_head = SWIG_GetModule(clientdata); + if (!module_head) { + /* This is the first module loaded for this interpreter */ + /* so set the swig module into the interpreter */ + SWIG_SetModule(clientdata, &swig_module); + } else { + /* the interpreter has loaded a SWIG module, but has it loaded this one? */ + iter=module_head; + do { + if (iter==&swig_module) { + /* Our module is already in the list, so there's nothing more to do. */ + return; + } + iter=iter->next; + } while (iter!= module_head); + + /* otherwise we must add our module into the list */ + swig_module.next = module_head->next; + module_head->next = &swig_module; + } + + /* When multiple interpreters are used, a module could have already been initialized in + a different interpreter, but not yet have a pointer in this interpreter. + In this case, we do not want to continue adding types... everything should be + set up already */ + if (init == 0) return; + + /* Now work on filling in swig_module.types */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: size %lu\n", (unsigned long)swig_module.size); +#endif + for (i = 0; i < swig_module.size; ++i) { + swig_type_info *type = 0; + swig_type_info *ret; + swig_cast_info *cast; + +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); +#endif + + /* if there is another module already loaded */ + if (swig_module.next != &swig_module) { + type = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, swig_module.type_initial[i]->name); + } + if (type) { + /* Overwrite clientdata field */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found type %s\n", type->name); +#endif + if (swig_module.type_initial[i]->clientdata) { + type->clientdata = swig_module.type_initial[i]->clientdata; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found and overwrite type %s \n", type->name); +#endif + } + } else { + type = swig_module.type_initial[i]; + } + + /* Insert casting types */ + cast = swig_module.cast_initial[i]; + while (cast->type) { + /* Don't need to add information already in the list */ + ret = 0; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: look cast %s\n", cast->type->name); +#endif + if (swig_module.next != &swig_module) { + ret = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, cast->type->name); +#ifdef SWIGRUNTIME_DEBUG + if (ret) printf("SWIG_InitializeModule: found cast %s\n", ret->name); +#endif + } + if (ret) { + if (type == swig_module.type_initial[i]) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: skip old type %s\n", ret->name); +#endif + cast->type = ret; + ret = 0; + } else { + /* Check for casting already in the list */ + swig_cast_info *ocast = SWIG_TypeCheck(ret->name, type); +#ifdef SWIGRUNTIME_DEBUG + if (ocast) printf("SWIG_InitializeModule: skip old cast %s\n", ret->name); +#endif + if (!ocast) ret = 0; + } + } + + if (!ret) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: adding cast %s\n", cast->type->name); +#endif + if (type->cast) { + type->cast->prev = cast; + cast->next = type->cast; + } + type->cast = cast; + } + cast++; + } + /* Set entry in modules->types array equal to the type */ + swig_module.types[i] = type; + } + swig_module.types[i] = 0; + +#ifdef SWIGRUNTIME_DEBUG + printf("**** SWIG_InitializeModule: Cast List ******\n"); + for (i = 0; i < swig_module.size; ++i) { + int j = 0; + swig_cast_info *cast = swig_module.cast_initial[i]; + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); + while (cast->type) { + printf("SWIG_InitializeModule: cast type %s\n", cast->type->name); + cast++; + ++j; + } + printf("---- Total casts: %d\n",j); + } + printf("**** SWIG_InitializeModule: Cast List ******\n"); +#endif +} + +/* This function will propagate the clientdata field of type to +* any new swig_type_info structures that have been added into the list +* of equivalent types. It is like calling +* SWIG_TypeClientData(type, clientdata) a second time. +*/ +SWIGRUNTIME void +SWIG_PropagateClientData(void) { + size_t i; + swig_cast_info *equiv; + static int init_run = 0; + + if (init_run) return; + init_run = 1; + + for (i = 0; i < swig_module.size; i++) { + if (swig_module.types[i]->clientdata) { + equiv = swig_module.types[i]->cast; + while (equiv) { + if (!equiv->converter) { + if (equiv->type && !equiv->type->clientdata) + SWIG_TypeClientData(equiv->type, swig_module.types[i]->clientdata); + } + equiv = equiv->next; + } + } + } +} + +#ifdef __cplusplus +#if 0 +{ + /* c-mode */ +#endif +} +#endif + + + +#ifdef __cplusplus +extern "C" { +#endif + + /* Python-specific SWIG API */ +#define SWIG_newvarlink() SWIG_Python_newvarlink() +#define SWIG_addvarlink(p, name, get_attr, set_attr) SWIG_Python_addvarlink(p, name, get_attr, set_attr) +#define SWIG_InstallConstants(d, constants) SWIG_Python_InstallConstants(d, constants) + + /* ----------------------------------------------------------------------------- + * global variable support code. + * ----------------------------------------------------------------------------- */ + + typedef struct swig_globalvar { + char *name; /* Name of global variable */ + PyObject *(*get_attr)(void); /* Return the current value */ + int (*set_attr)(PyObject *); /* Set the value */ + struct swig_globalvar *next; + } swig_globalvar; + + typedef struct swig_varlinkobject { + PyObject_HEAD + swig_globalvar *vars; + } swig_varlinkobject; + + SWIGINTERN PyObject * + swig_varlink_repr(swig_varlinkobject *SWIGUNUSEDPARM(v)) { +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_InternFromString(""); +#else + return PyString_FromString(""); +#endif + } + + SWIGINTERN PyObject * + swig_varlink_str(swig_varlinkobject *v) { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *str = PyUnicode_InternFromString("("); + PyObject *tail; + PyObject *joined; + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + tail = PyUnicode_FromString(var->name); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + if (var->next) { + tail = PyUnicode_InternFromString(", "); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + } + } + tail = PyUnicode_InternFromString(")"); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; +#else + PyObject *str = PyString_FromString("("); + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + PyString_ConcatAndDel(&str,PyString_FromString(var->name)); + if (var->next) PyString_ConcatAndDel(&str,PyString_FromString(", ")); + } + PyString_ConcatAndDel(&str,PyString_FromString(")")); +#endif + return str; + } + + SWIGINTERN void + swig_varlink_dealloc(swig_varlinkobject *v) { + swig_globalvar *var = v->vars; + while (var) { + swig_globalvar *n = var->next; + free(var->name); + free(var); + var = n; + } + } + + SWIGINTERN PyObject * + swig_varlink_getattr(swig_varlinkobject *v, char *n) { + PyObject *res = NULL; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->get_attr)(); + break; + } + var = var->next; + } + if (res == NULL && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN int + swig_varlink_setattr(swig_varlinkobject *v, char *n, PyObject *p) { + int res = 1; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->set_attr)(p); + break; + } + var = var->next; + } + if (res == 1 && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN PyTypeObject* + swig_varlink_type(void) { + static char varlink__doc__[] = "Swig var link object"; + static PyTypeObject varlink_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "swigvarlink", /* tp_name */ + sizeof(swig_varlinkobject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor) swig_varlink_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc) swig_varlink_getattr, /* tp_getattr */ + (setattrfunc) swig_varlink_setattr, /* tp_setattr */ + 0, /* tp_compare */ + (reprfunc) swig_varlink_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + 0, /* tp_hash */ + 0, /* tp_call */ + (reprfunc) swig_varlink_str, /* tp_str */ + 0, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + 0, /* tp_flags */ + varlink__doc__, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, /* tp_iter -> tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + varlink_type = tmp; + type_init = 1; + if (PyType_Ready(&varlink_type) < 0) + return NULL; + } + return &varlink_type; + } + + /* Create a variable linking object for use later */ + SWIGINTERN PyObject * + SWIG_Python_newvarlink(void) { + swig_varlinkobject *result = PyObject_NEW(swig_varlinkobject, swig_varlink_type()); + if (result) { + result->vars = 0; + } + return ((PyObject*) result); + } + + SWIGINTERN void + SWIG_Python_addvarlink(PyObject *p, const char *name, PyObject *(*get_attr)(void), int (*set_attr)(PyObject *p)) { + swig_varlinkobject *v = (swig_varlinkobject *) p; + swig_globalvar *gv = (swig_globalvar *) malloc(sizeof(swig_globalvar)); + if (gv) { + size_t size = strlen(name)+1; + gv->name = (char *)malloc(size); + if (gv->name) { + memcpy(gv->name, name, size); + gv->get_attr = get_attr; + gv->set_attr = set_attr; + gv->next = v->vars; + } + } + v->vars = gv; + } + + SWIGINTERN PyObject * + SWIG_globals(void) { + static PyObject *globals = 0; + if (!globals) { + globals = SWIG_newvarlink(); + } + return globals; + } + + /* ----------------------------------------------------------------------------- + * constants/methods manipulation + * ----------------------------------------------------------------------------- */ + + /* Install Constants */ + SWIGINTERN void + SWIG_Python_InstallConstants(PyObject *d, swig_const_info constants[]) { + PyObject *obj = 0; + size_t i; + for (i = 0; constants[i].type; ++i) { + switch(constants[i].type) { + case SWIG_PY_POINTER: + obj = SWIG_InternalNewPointerObj(constants[i].pvalue, *(constants[i]).ptype,0); + break; + case SWIG_PY_BINARY: + obj = SWIG_NewPackedObj(constants[i].pvalue, constants[i].lvalue, *(constants[i].ptype)); + break; + default: + obj = 0; + break; + } + if (obj) { + PyDict_SetItemString(d, constants[i].name, obj); + Py_DECREF(obj); + } + } + } + + /* -----------------------------------------------------------------------------*/ + /* Fix SwigMethods to carry the callback ptrs when needed */ + /* -----------------------------------------------------------------------------*/ + + SWIGINTERN void + SWIG_Python_FixMethods(PyMethodDef *methods, + swig_const_info *const_table, + swig_type_info **types, + swig_type_info **types_initial) { + size_t i; + for (i = 0; methods[i].ml_name; ++i) { + const char *c = methods[i].ml_doc; + if (!c) continue; + c = strstr(c, "swig_ptr: "); + if (c) { + int j; + swig_const_info *ci = 0; + const char *name = c + 10; + for (j = 0; const_table[j].type; ++j) { + if (strncmp(const_table[j].name, name, + strlen(const_table[j].name)) == 0) { + ci = &(const_table[j]); + break; + } + } + if (ci) { + void *ptr = (ci->type == SWIG_PY_POINTER) ? ci->pvalue : 0; + if (ptr) { + size_t shift = (ci->ptype) - types; + swig_type_info *ty = types_initial[shift]; + size_t ldoc = (c - methods[i].ml_doc); + size_t lptr = strlen(ty->name)+2*sizeof(void*)+2; + char *ndoc = (char*)malloc(ldoc + lptr + 10); + if (ndoc) { + char *buff = ndoc; + memcpy(buff, methods[i].ml_doc, ldoc); + buff += ldoc; + memcpy(buff, "swig_ptr: ", 10); + buff += 10; + SWIG_PackVoidPtr(buff, ptr, ty->name, lptr); + methods[i].ml_doc = ndoc; + } + } + } + } + } + } + + /* ----------------------------------------------------------------------------- + * Method creation and docstring support functions + * ----------------------------------------------------------------------------- */ + + /* ----------------------------------------------------------------------------- + * Function to find the method definition with the correct docstring for the + * proxy module as opposed to the low-level API + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name) { + /* Find the function in the modified method table */ + size_t offset = 0; + int found = 0; + while (SwigMethods_proxydocs[offset].ml_meth != NULL) { + if (strcmp(SwigMethods_proxydocs[offset].ml_name, name) == 0) { + found = 1; + break; + } + offset++; + } + /* Use the copy with the modified docstring if available */ + return found ? &SwigMethods_proxydocs[offset] : NULL; + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyInstanceMethod_New() used in Python 3 + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } +#if PY_VERSION_HEX >= 0x03000000 + return PyInstanceMethod_New(func); +#else + return PyMethod_New(func, NULL, NULL); +#endif + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyStaticMethod_New() + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } + return PyStaticMethod_New(func); + } + +#ifdef __cplusplus +} +#endif + +/* -----------------------------------------------------------------------------* + * Partial Init method + * -----------------------------------------------------------------------------*/ + +#ifdef __cplusplus +extern "C" +#endif + +SWIGEXPORT +#if PY_VERSION_HEX >= 0x03000000 +PyObject* +#else +void +#endif +SWIG_init(void) { + PyObject *m, *d, *md, *globals; + +#if PY_VERSION_HEX >= 0x03000000 + static struct PyModuleDef SWIG_module = { + PyModuleDef_HEAD_INIT, + SWIG_name, + NULL, + -1, + SwigMethods, + NULL, + NULL, + NULL, + NULL + }; +#endif + +#if defined(SWIGPYTHON_BUILTIN) + static SwigPyClientData SwigPyObject_clientdata = { + 0, 0, 0, 0, 0, 0, 0 + }; + static PyGetSetDef this_getset_def = { + (char *)"this", &SwigPyBuiltin_ThisClosure, NULL, NULL, NULL + }; + static SwigPyGetSet thisown_getset_closure = { + SwigPyObject_own, + SwigPyObject_own + }; + static PyGetSetDef thisown_getset_def = { + (char *)"thisown", SwigPyBuiltin_GetterClosure, SwigPyBuiltin_SetterClosure, NULL, &thisown_getset_closure + }; + PyTypeObject *builtin_pytype; + int builtin_base_count; + swig_type_info *builtin_basetype; + PyObject *tuple; + PyGetSetDescrObject *static_getset; + PyTypeObject *metatype; + PyTypeObject *swigpyobject; + SwigPyClientData *cd; + PyObject *public_interface, *public_symbol; + PyObject *this_descr; + PyObject *thisown_descr; + PyObject *self = 0; + int i; + + (void)builtin_pytype; + (void)builtin_base_count; + (void)builtin_basetype; + (void)tuple; + (void)static_getset; + (void)self; + + /* Metaclass is used to implement static member variables */ + metatype = SwigPyObjectType(); + assert(metatype); +#endif + + (void)globals; + + /* Create singletons now to avoid potential deadlocks with multi-threaded usage after module initialization */ + SWIG_This(); + SWIG_Python_TypeCache(); + SwigPyPacked_type(); +#ifndef SWIGPYTHON_BUILTIN + SwigPyObject_type(); +#endif + + /* Fix SwigMethods to carry the callback ptrs when needed */ + SWIG_Python_FixMethods(SwigMethods, swig_const_table, swig_types, swig_type_initial); + +#if PY_VERSION_HEX >= 0x03000000 + m = PyModule_Create(&SWIG_module); +#else + m = Py_InitModule(SWIG_name, SwigMethods); +#endif + + md = d = PyModule_GetDict(m); + (void)md; + + SWIG_InitializeModule(0); + +#ifdef SWIGPYTHON_BUILTIN + swigpyobject = SwigPyObject_TypeOnce(); + + SwigPyObject_stype = SWIG_MangledTypeQuery("_p_SwigPyObject"); + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + if (!cd) { + SwigPyObject_stype->clientdata = &SwigPyObject_clientdata; + SwigPyObject_clientdata.pytype = swigpyobject; + } else if (swigpyobject->tp_basicsize != cd->pytype->tp_basicsize) { + PyErr_SetString(PyExc_RuntimeError, "Import error: attempted to load two incompatible swig-generated modules."); +# if PY_VERSION_HEX >= 0x03000000 + return NULL; +# else + return; +# endif + } + + /* All objects have a 'this' attribute */ + this_descr = PyDescr_NewGetSet(SwigPyObject_type(), &this_getset_def); + (void)this_descr; + + /* All objects have a 'thisown' attribute */ + thisown_descr = PyDescr_NewGetSet(SwigPyObject_type(), &thisown_getset_def); + (void)thisown_descr; + + public_interface = PyList_New(0); + public_symbol = 0; + (void)public_symbol; + + PyDict_SetItemString(md, "__all__", public_interface); + Py_DECREF(public_interface); + for (i = 0; SwigMethods[i].ml_name != NULL; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, SwigMethods[i].ml_name); + for (i = 0; swig_const_table[i].name != 0; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, swig_const_table[i].name); +#endif + + SWIG_InstallConstants(d,swig_const_table); + + SWIG_Python_SetConstant(d, "Driver_INDICATOR_OFF",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_OFF))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_RIGHT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_RIGHT))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_LEFT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_LEFT))); + SWIG_Python_SetConstant(d, "Driver_SPEED",SWIG_From_int(static_cast< int >(webots::Driver::SPEED))); + SWIG_Python_SetConstant(d, "Driver_TORQUE",SWIG_From_int(static_cast< int >(webots::Driver::TORQUE))); + SWIG_Python_SetConstant(d, "Driver_DOWN",SWIG_From_int(static_cast< int >(webots::Driver::DOWN))); + SWIG_Python_SetConstant(d, "Driver_SLOW",SWIG_From_int(static_cast< int >(webots::Driver::SLOW))); + SWIG_Python_SetConstant(d, "Driver_NORMAL",SWIG_From_int(static_cast< int >(webots::Driver::NORMAL))); + SWIG_Python_SetConstant(d, "Driver_FAST",SWIG_From_int(static_cast< int >(webots::Driver::FAST))); + SWIG_Python_SetConstant(d, "Car_TRACTION",SWIG_From_int(static_cast< int >(webots::Car::TRACTION))); + SWIG_Python_SetConstant(d, "Car_PROPULSION",SWIG_From_int(static_cast< int >(webots::Car::PROPULSION))); + SWIG_Python_SetConstant(d, "Car_FOUR_BY_FOUR",SWIG_From_int(static_cast< int >(webots::Car::FOUR_BY_FOUR))); + SWIG_Python_SetConstant(d, "Car_COMBUSTION_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::COMBUSTION_ENGINE))); + SWIG_Python_SetConstant(d, "Car_ELECTRIC_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::ELECTRIC_ENGINE))); + SWIG_Python_SetConstant(d, "Car_PARALLEL_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::PARALLEL_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_POWER_SPLIT_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::POWER_SPLIT_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_NB",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_NB))); +#if PY_VERSION_HEX >= 0x03000000 + return m; +#else + return; +#endif +} + diff --git a/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle39.cpp b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle39.cpp new file mode 100644 index 000000000..cbfd22412 --- /dev/null +++ b/webots_ros2_driver/webots/projects/default/libraries/vehicle/python/vehicle39.cpp @@ -0,0 +1,5255 @@ +/* ---------------------------------------------------------------------------- + * This file was automatically generated by SWIG (http://www.swig.org). + * Version 4.0.2 + * + * This file is not intended to be easily readable and contains a number of + * coding conventions designed to improve portability and efficiency. Do not make + * changes to this file unless you know what you are doing--modify the SWIG + * interface file instead. + * ----------------------------------------------------------------------------- */ + + +#ifndef SWIGPYTHON +#define SWIGPYTHON +#endif + +#define SWIG_PYTHON_DIRECTOR_NO_VTABLE + + +#ifdef __cplusplus +/* SwigValueWrapper is described in swig.swg */ +template class SwigValueWrapper { + struct SwigMovePointer { + T *ptr; + SwigMovePointer(T *p) : ptr(p) { } + ~SwigMovePointer() { delete ptr; } + SwigMovePointer& operator=(SwigMovePointer& rhs) { T* oldptr = ptr; ptr = 0; delete oldptr; ptr = rhs.ptr; rhs.ptr = 0; return *this; } + } pointer; + SwigValueWrapper& operator=(const SwigValueWrapper& rhs); + SwigValueWrapper(const SwigValueWrapper& rhs); +public: + SwigValueWrapper() : pointer(0) { } + SwigValueWrapper& operator=(const T& t) { SwigMovePointer tmp(new T(t)); pointer = tmp; return *this; } + operator T&() const { return *pointer.ptr; } + T *operator&() { return pointer.ptr; } +}; + +template T SwigValueInit() { + return T(); +} +#endif + +/* ----------------------------------------------------------------------------- + * This section contains generic SWIG labels for method/variable + * declarations/attributes, and other compiler dependent labels. + * ----------------------------------------------------------------------------- */ + +/* template workaround for compilers that cannot correctly implement the C++ standard */ +#ifndef SWIGTEMPLATEDISAMBIGUATOR +# if defined(__SUNPRO_CC) && (__SUNPRO_CC <= 0x560) +# define SWIGTEMPLATEDISAMBIGUATOR template +# elif defined(__HP_aCC) +/* Needed even with `aCC -AA' when `aCC -V' reports HP ANSI C++ B3910B A.03.55 */ +/* If we find a maximum version that requires this, the test would be __HP_aCC <= 35500 for A.03.55 */ +# define SWIGTEMPLATEDISAMBIGUATOR template +# else +# define SWIGTEMPLATEDISAMBIGUATOR +# endif +#endif + +/* inline attribute */ +#ifndef SWIGINLINE +# if defined(__cplusplus) || (defined(__GNUC__) && !defined(__STRICT_ANSI__)) +# define SWIGINLINE inline +# else +# define SWIGINLINE +# endif +#endif + +/* attribute recognised by some compilers to avoid 'unused' warnings */ +#ifndef SWIGUNUSED +# if defined(__GNUC__) +# if !(defined(__cplusplus)) || (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +# elif defined(__ICC) +# define SWIGUNUSED __attribute__ ((__unused__)) +# else +# define SWIGUNUSED +# endif +#endif + +#ifndef SWIG_MSC_UNSUPPRESS_4505 +# if defined(_MSC_VER) +# pragma warning(disable : 4505) /* unreferenced local function has been removed */ +# endif +#endif + +#ifndef SWIGUNUSEDPARM +# ifdef __cplusplus +# define SWIGUNUSEDPARM(p) +# else +# define SWIGUNUSEDPARM(p) p SWIGUNUSED +# endif +#endif + +/* internal SWIG method */ +#ifndef SWIGINTERN +# define SWIGINTERN static SWIGUNUSED +#endif + +/* internal inline SWIG method */ +#ifndef SWIGINTERNINLINE +# define SWIGINTERNINLINE SWIGINTERN SWIGINLINE +#endif + +/* exporting methods */ +#if defined(__GNUC__) +# if (__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4) +# ifndef GCC_HASCLASSVISIBILITY +# define GCC_HASCLASSVISIBILITY +# endif +# endif +#endif + +#ifndef SWIGEXPORT +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# if defined(STATIC_LINKED) +# define SWIGEXPORT +# else +# define SWIGEXPORT __declspec(dllexport) +# endif +# else +# if defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) +# define SWIGEXPORT __attribute__ ((visibility("default"))) +# else +# define SWIGEXPORT +# endif +# endif +#endif + +/* calling conventions for Windows */ +#ifndef SWIGSTDCALL +# if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) +# define SWIGSTDCALL __stdcall +# else +# define SWIGSTDCALL +# endif +#endif + +/* Deal with Microsoft's attempt at deprecating C standard runtime functions */ +#if !defined(SWIG_NO_CRT_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_CRT_SECURE_NO_DEPRECATE) +# define _CRT_SECURE_NO_DEPRECATE +#endif + +/* Deal with Microsoft's attempt at deprecating methods in the standard C++ library */ +#if !defined(SWIG_NO_SCL_SECURE_NO_DEPRECATE) && defined(_MSC_VER) && !defined(_SCL_SECURE_NO_DEPRECATE) +# define _SCL_SECURE_NO_DEPRECATE +#endif + +/* Deal with Apple's deprecated 'AssertMacros.h' from Carbon-framework */ +#if defined(__APPLE__) && !defined(__ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES) +# define __ASSERT_MACROS_DEFINE_VERSIONS_WITHOUT_UNDERSCORES 0 +#endif + +/* Intel's compiler complains if a variable which was never initialised is + * cast to void, which is a common idiom which we use to indicate that we + * are aware a variable isn't used. So we just silence that warning. + * See: https://github.com/swig/swig/issues/192 for more discussion. + */ +#ifdef __INTEL_COMPILER +# pragma warning disable 592 +#endif + + +#if defined(__GNUC__) && defined(_WIN32) && !defined(SWIG_PYTHON_NO_HYPOT_WORKAROUND) +/* Workaround for '::hypot' has not been declared', see https://bugs.python.org/issue11566 */ +# include +#endif + +#if defined(_DEBUG) && defined(SWIG_PYTHON_INTERPRETER_NO_DEBUG) +/* Use debug wrappers with the Python release dll */ +# undef _DEBUG +# include +# define _DEBUG 1 +#else +# include +#endif + +/* ----------------------------------------------------------------------------- + * swigrun.swg + * + * This file contains generic C API SWIG runtime support for pointer + * type checking. + * ----------------------------------------------------------------------------- */ + +/* This should only be incremented when either the layout of swig_type_info changes, + or for whatever reason, the runtime changes incompatibly */ +#define SWIG_RUNTIME_VERSION "4" + +/* define SWIG_TYPE_TABLE_NAME as "SWIG_TYPE_TABLE" */ +#ifdef SWIG_TYPE_TABLE +# define SWIG_QUOTE_STRING(x) #x +# define SWIG_EXPAND_AND_QUOTE_STRING(x) SWIG_QUOTE_STRING(x) +# define SWIG_TYPE_TABLE_NAME SWIG_EXPAND_AND_QUOTE_STRING(SWIG_TYPE_TABLE) +#else +# define SWIG_TYPE_TABLE_NAME +#endif + +/* + You can use the SWIGRUNTIME and SWIGRUNTIMEINLINE macros for + creating a static or dynamic library from the SWIG runtime code. + In 99.9% of the cases, SWIG just needs to declare them as 'static'. + + But only do this if strictly necessary, ie, if you have problems + with your compiler or suchlike. +*/ + +#ifndef SWIGRUNTIME +# define SWIGRUNTIME SWIGINTERN +#endif + +#ifndef SWIGRUNTIMEINLINE +# define SWIGRUNTIMEINLINE SWIGRUNTIME SWIGINLINE +#endif + +/* Generic buffer size */ +#ifndef SWIG_BUFFER_SIZE +# define SWIG_BUFFER_SIZE 1024 +#endif + +/* Flags for pointer conversions */ +#define SWIG_POINTER_DISOWN 0x1 +#define SWIG_CAST_NEW_MEMORY 0x2 +#define SWIG_POINTER_NO_NULL 0x4 + +/* Flags for new pointer objects */ +#define SWIG_POINTER_OWN 0x1 + + +/* + Flags/methods for returning states. + + The SWIG conversion methods, as ConvertPtr, return an integer + that tells if the conversion was successful or not. And if not, + an error code can be returned (see swigerrors.swg for the codes). + + Use the following macros/flags to set or process the returning + states. + + In old versions of SWIG, code such as the following was usually written: + + if (SWIG_ConvertPtr(obj,vptr,ty.flags) != -1) { + // success code + } else { + //fail code + } + + Now you can be more explicit: + + int res = SWIG_ConvertPtr(obj,vptr,ty.flags); + if (SWIG_IsOK(res)) { + // success code + } else { + // fail code + } + + which is the same really, but now you can also do + + Type *ptr; + int res = SWIG_ConvertPtr(obj,(void **)(&ptr),ty.flags); + if (SWIG_IsOK(res)) { + // success code + if (SWIG_IsNewObj(res) { + ... + delete *ptr; + } else { + ... + } + } else { + // fail code + } + + I.e., now SWIG_ConvertPtr can return new objects and you can + identify the case and take care of the deallocation. Of course that + also requires SWIG_ConvertPtr to return new result values, such as + + int SWIG_ConvertPtr(obj, ptr,...) { + if () { + if () { + *ptr = ; + return SWIG_NEWOBJ; + } else { + *ptr = ; + return SWIG_OLDOBJ; + } + } else { + return SWIG_BADOBJ; + } + } + + Of course, returning the plain '0(success)/-1(fail)' still works, but you can be + more explicit by returning SWIG_BADOBJ, SWIG_ERROR or any of the + SWIG errors code. + + Finally, if the SWIG_CASTRANK_MODE is enabled, the result code + allows to return the 'cast rank', for example, if you have this + + int food(double) + int fooi(int); + + and you call + + food(1) // cast rank '1' (1 -> 1.0) + fooi(1) // cast rank '0' + + just use the SWIG_AddCast()/SWIG_CheckState() +*/ + +#define SWIG_OK (0) +#define SWIG_ERROR (-1) +#define SWIG_IsOK(r) (r >= 0) +#define SWIG_ArgError(r) ((r != SWIG_ERROR) ? r : SWIG_TypeError) + +/* The CastRankLimit says how many bits are used for the cast rank */ +#define SWIG_CASTRANKLIMIT (1 << 8) +/* The NewMask denotes the object was created (using new/malloc) */ +#define SWIG_NEWOBJMASK (SWIG_CASTRANKLIMIT << 1) +/* The TmpMask is for in/out typemaps that use temporal objects */ +#define SWIG_TMPOBJMASK (SWIG_NEWOBJMASK << 1) +/* Simple returning values */ +#define SWIG_BADOBJ (SWIG_ERROR) +#define SWIG_OLDOBJ (SWIG_OK) +#define SWIG_NEWOBJ (SWIG_OK | SWIG_NEWOBJMASK) +#define SWIG_TMPOBJ (SWIG_OK | SWIG_TMPOBJMASK) +/* Check, add and del mask methods */ +#define SWIG_AddNewMask(r) (SWIG_IsOK(r) ? (r | SWIG_NEWOBJMASK) : r) +#define SWIG_DelNewMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_NEWOBJMASK) : r) +#define SWIG_IsNewObj(r) (SWIG_IsOK(r) && (r & SWIG_NEWOBJMASK)) +#define SWIG_AddTmpMask(r) (SWIG_IsOK(r) ? (r | SWIG_TMPOBJMASK) : r) +#define SWIG_DelTmpMask(r) (SWIG_IsOK(r) ? (r & ~SWIG_TMPOBJMASK) : r) +#define SWIG_IsTmpObj(r) (SWIG_IsOK(r) && (r & SWIG_TMPOBJMASK)) + +/* Cast-Rank Mode */ +#if defined(SWIG_CASTRANK_MODE) +# ifndef SWIG_TypeRank +# define SWIG_TypeRank unsigned long +# endif +# ifndef SWIG_MAXCASTRANK /* Default cast allowed */ +# define SWIG_MAXCASTRANK (2) +# endif +# define SWIG_CASTRANKMASK ((SWIG_CASTRANKLIMIT) -1) +# define SWIG_CastRank(r) (r & SWIG_CASTRANKMASK) +SWIGINTERNINLINE int SWIG_AddCast(int r) { + return SWIG_IsOK(r) ? ((SWIG_CastRank(r) < SWIG_MAXCASTRANK) ? (r + 1) : SWIG_ERROR) : r; +} +SWIGINTERNINLINE int SWIG_CheckState(int r) { + return SWIG_IsOK(r) ? SWIG_CastRank(r) + 1 : 0; +} +#else /* no cast-rank mode */ +# define SWIG_AddCast(r) (r) +# define SWIG_CheckState(r) (SWIG_IsOK(r) ? 1 : 0) +#endif + + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef void *(*swig_converter_func)(void *, int *); +typedef struct swig_type_info *(*swig_dycast_func)(void **); + +/* Structure to store information on one type */ +typedef struct swig_type_info { + const char *name; /* mangled name of this type */ + const char *str; /* human readable name of this type */ + swig_dycast_func dcast; /* dynamic cast function down a hierarchy */ + struct swig_cast_info *cast; /* linked list of types that can cast into this type */ + void *clientdata; /* language specific type data */ + int owndata; /* flag if the structure owns the clientdata */ +} swig_type_info; + +/* Structure to store a type and conversion function used for casting */ +typedef struct swig_cast_info { + swig_type_info *type; /* pointer to type that is equivalent to this type */ + swig_converter_func converter; /* function to cast the void pointers */ + struct swig_cast_info *next; /* pointer to next cast in linked list */ + struct swig_cast_info *prev; /* pointer to the previous cast */ +} swig_cast_info; + +/* Structure used to store module information + * Each module generates one structure like this, and the runtime collects + * all of these structures and stores them in a circularly linked list.*/ +typedef struct swig_module_info { + swig_type_info **types; /* Array of pointers to swig_type_info structures that are in this module */ + size_t size; /* Number of types in this module */ + struct swig_module_info *next; /* Pointer to next element in circularly linked list */ + swig_type_info **type_initial; /* Array of initially generated type structures */ + swig_cast_info **cast_initial; /* Array of initially generated casting structures */ + void *clientdata; /* Language specific module data */ +} swig_module_info; + +/* + Compare two type names skipping the space characters, therefore + "char*" == "char *" and "Class" == "Class", etc. + + Return 0 when the two name types are equivalent, as in + strncmp, but skipping ' '. +*/ +SWIGRUNTIME int +SWIG_TypeNameComp(const char *f1, const char *l1, + const char *f2, const char *l2) { + for (;(f1 != l1) && (f2 != l2); ++f1, ++f2) { + while ((*f1 == ' ') && (f1 != l1)) ++f1; + while ((*f2 == ' ') && (f2 != l2)) ++f2; + if (*f1 != *f2) return (*f1 > *f2) ? 1 : -1; + } + return (int)((l1 - f1) - (l2 - f2)); +} + +/* + Check type equivalence in a name list like ||... + Return 0 if equal, -1 if nb < tb, 1 if nb > tb +*/ +SWIGRUNTIME int +SWIG_TypeCmp(const char *nb, const char *tb) { + int equiv = 1; + const char* te = tb + strlen(tb); + const char* ne = nb; + while (equiv != 0 && *ne) { + for (nb = ne; *ne; ++ne) { + if (*ne == '|') break; + } + equiv = SWIG_TypeNameComp(nb, ne, tb, te); + if (*ne) ++ne; + } + return equiv; +} + +/* + Check type equivalence in a name list like ||... + Return 0 if not equal, 1 if equal +*/ +SWIGRUNTIME int +SWIG_TypeEquiv(const char *nb, const char *tb) { + return SWIG_TypeCmp(nb, tb) == 0 ? 1 : 0; +} + +/* + Check the typename +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheck(const char *c, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (strcmp(iter->type->name, c) == 0) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Identical to SWIG_TypeCheck, except strcmp is replaced with a pointer comparison +*/ +SWIGRUNTIME swig_cast_info * +SWIG_TypeCheckStruct(swig_type_info *from, swig_type_info *ty) { + if (ty) { + swig_cast_info *iter = ty->cast; + while (iter) { + if (iter->type == from) { + if (iter == ty->cast) + return iter; + /* Move iter to the top of the linked list */ + iter->prev->next = iter->next; + if (iter->next) + iter->next->prev = iter->prev; + iter->next = ty->cast; + iter->prev = 0; + if (ty->cast) ty->cast->prev = iter; + ty->cast = iter; + return iter; + } + iter = iter->next; + } + } + return 0; +} + +/* + Cast a pointer up an inheritance hierarchy +*/ +SWIGRUNTIMEINLINE void * +SWIG_TypeCast(swig_cast_info *ty, void *ptr, int *newmemory) { + return ((!ty) || (!ty->converter)) ? ptr : (*ty->converter)(ptr, newmemory); +} + +/* + Dynamic pointer casting. Down an inheritance hierarchy +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeDynamicCast(swig_type_info *ty, void **ptr) { + swig_type_info *lastty = ty; + if (!ty || !ty->dcast) return ty; + while (ty && (ty->dcast)) { + ty = (*ty->dcast)(ptr); + if (ty) lastty = ty; + } + return lastty; +} + +/* + Return the name associated with this type +*/ +SWIGRUNTIMEINLINE const char * +SWIG_TypeName(const swig_type_info *ty) { + return ty->name; +} + +/* + Return the pretty name associated with this type, + that is an unmangled type name in a form presentable to the user. +*/ +SWIGRUNTIME const char * +SWIG_TypePrettyName(const swig_type_info *type) { + /* The "str" field contains the equivalent pretty names of the + type, separated by vertical-bar characters. We choose + to print the last name, as it is often (?) the most + specific. */ + if (!type) return NULL; + if (type->str != NULL) { + const char *last_name = type->str; + const char *s; + for (s = type->str; *s; s++) + if (*s == '|') last_name = s+1; + return last_name; + } + else + return type->name; +} + +/* + Set the clientdata field for a type +*/ +SWIGRUNTIME void +SWIG_TypeClientData(swig_type_info *ti, void *clientdata) { + swig_cast_info *cast = ti->cast; + /* if (ti->clientdata == clientdata) return; */ + ti->clientdata = clientdata; + + while (cast) { + if (!cast->converter) { + swig_type_info *tc = cast->type; + if (!tc->clientdata) { + SWIG_TypeClientData(tc, clientdata); + } + } + cast = cast->next; + } +} +SWIGRUNTIME void +SWIG_TypeNewClientData(swig_type_info *ti, void *clientdata) { + SWIG_TypeClientData(ti, clientdata); + ti->owndata = 1; +} + +/* + Search for a swig_type_info structure only by mangled name + Search is a O(log #types) + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_MangledTypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + swig_module_info *iter = start; + do { + if (iter->size) { + size_t l = 0; + size_t r = iter->size - 1; + do { + /* since l+r >= 0, we can (>> 1) instead (/ 2) */ + size_t i = (l + r) >> 1; + const char *iname = iter->types[i]->name; + if (iname) { + int compare = strcmp(name, iname); + if (compare == 0) { + return iter->types[i]; + } else if (compare < 0) { + if (i) { + r = i - 1; + } else { + break; + } + } else if (compare > 0) { + l = i + 1; + } + } else { + break; /* should never happen */ + } + } while (l <= r); + } + iter = iter->next; + } while (iter != end); + return 0; +} + +/* + Search for a swig_type_info structure for either a mangled name or a human readable name. + It first searches the mangled names of the types, which is a O(log #types) + If a type is not found it then searches the human readable names, which is O(#types). + + We start searching at module start, and finish searching when start == end. + Note: if start == end at the beginning of the function, we go all the way around + the circular list. +*/ +SWIGRUNTIME swig_type_info * +SWIG_TypeQueryModule(swig_module_info *start, + swig_module_info *end, + const char *name) { + /* STEP 1: Search the name field using binary search */ + swig_type_info *ret = SWIG_MangledTypeQueryModule(start, end, name); + if (ret) { + return ret; + } else { + /* STEP 2: If the type hasn't been found, do a complete search + of the str field (the human readable name) */ + swig_module_info *iter = start; + do { + size_t i = 0; + for (; i < iter->size; ++i) { + if (iter->types[i]->str && (SWIG_TypeEquiv(iter->types[i]->str, name))) + return iter->types[i]; + } + iter = iter->next; + } while (iter != end); + } + + /* neither found a match */ + return 0; +} + +/* + Pack binary data into a string +*/ +SWIGRUNTIME char * +SWIG_PackData(char *c, void *ptr, size_t sz) { + static const char hex[17] = "0123456789abcdef"; + const unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + unsigned char uu = *u; + *(c++) = hex[(uu & 0xf0) >> 4]; + *(c++) = hex[uu & 0xf]; + } + return c; +} + +/* + Unpack binary data from a string +*/ +SWIGRUNTIME const char * +SWIG_UnpackData(const char *c, void *ptr, size_t sz) { + unsigned char *u = (unsigned char *) ptr; + const unsigned char *eu = u + sz; + for (; u != eu; ++u) { + char d = *(c++); + unsigned char uu; + if ((d >= '0') && (d <= '9')) + uu = (unsigned char)((d - '0') << 4); + else if ((d >= 'a') && (d <= 'f')) + uu = (unsigned char)((d - ('a'-10)) << 4); + else + return (char *) 0; + d = *(c++); + if ((d >= '0') && (d <= '9')) + uu |= (unsigned char)(d - '0'); + else if ((d >= 'a') && (d <= 'f')) + uu |= (unsigned char)(d - ('a'-10)); + else + return (char *) 0; + *u = uu; + } + return c; +} + +/* + Pack 'void *' into a string buffer. +*/ +SWIGRUNTIME char * +SWIG_PackVoidPtr(char *buff, void *ptr, const char *name, size_t bsz) { + char *r = buff; + if ((2*sizeof(void *) + 2) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,&ptr,sizeof(void *)); + if (strlen(name) + 1 > (bsz - (r - buff))) return 0; + strcpy(r,name); + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackVoidPtr(const char *c, void **ptr, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + *ptr = (void *) 0; + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sizeof(void *)); +} + +SWIGRUNTIME char * +SWIG_PackDataName(char *buff, void *ptr, size_t sz, const char *name, size_t bsz) { + char *r = buff; + size_t lname = (name ? strlen(name) : 0); + if ((2*sz + 2 + lname) > bsz) return 0; + *(r++) = '_'; + r = SWIG_PackData(r,ptr,sz); + if (lname) { + strncpy(r,name,lname+1); + } else { + *r = 0; + } + return buff; +} + +SWIGRUNTIME const char * +SWIG_UnpackDataName(const char *c, void *ptr, size_t sz, const char *name) { + if (*c != '_') { + if (strcmp(c,"NULL") == 0) { + memset(ptr,0,sz); + return name; + } else { + return 0; + } + } + return SWIG_UnpackData(++c,ptr,sz); +} + +#ifdef __cplusplus +} +#endif + +/* Errors in SWIG */ +#define SWIG_UnknownError -1 +#define SWIG_IOError -2 +#define SWIG_RuntimeError -3 +#define SWIG_IndexError -4 +#define SWIG_TypeError -5 +#define SWIG_DivisionByZero -6 +#define SWIG_OverflowError -7 +#define SWIG_SyntaxError -8 +#define SWIG_ValueError -9 +#define SWIG_SystemError -10 +#define SWIG_AttributeError -11 +#define SWIG_MemoryError -12 +#define SWIG_NullReferenceError -13 + + + +/* Compatibility macros for Python 3 */ +#if PY_VERSION_HEX >= 0x03000000 + +#define PyClass_Check(obj) PyObject_IsInstance(obj, (PyObject *)&PyType_Type) +#define PyInt_Check(x) PyLong_Check(x) +#define PyInt_AsLong(x) PyLong_AsLong(x) +#define PyInt_FromLong(x) PyLong_FromLong(x) +#define PyInt_FromSize_t(x) PyLong_FromSize_t(x) +#define PyString_Check(name) PyBytes_Check(name) +#define PyString_FromString(x) PyUnicode_FromString(x) +#define PyString_Format(fmt, args) PyUnicode_Format(fmt, args) +#define PyString_AsString(str) PyBytes_AsString(str) +#define PyString_Size(str) PyBytes_Size(str) +#define PyString_InternFromString(key) PyUnicode_InternFromString(key) +#define Py_TPFLAGS_HAVE_CLASS Py_TPFLAGS_BASETYPE +#define PyString_AS_STRING(x) PyUnicode_AS_STRING(x) +#define _PyLong_FromSsize_t(x) PyLong_FromSsize_t(x) + +#endif + +#ifndef Py_TYPE +# define Py_TYPE(op) ((op)->ob_type) +#endif + +/* SWIG APIs for compatibility of both Python 2 & 3 */ + +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_Python_str_FromFormat PyUnicode_FromFormat +#else +# define SWIG_Python_str_FromFormat PyString_FromFormat +#endif + + +/* Warning: This function will allocate a new string in Python 3, + * so please call SWIG_Python_str_DelForPy3(x) to free the space. + */ +SWIGINTERN char* +SWIG_Python_str_AsChar(PyObject *str) +{ +#if PY_VERSION_HEX >= 0x03030000 + return (char *)PyUnicode_AsUTF8(str); +#elif PY_VERSION_HEX >= 0x03000000 + char *newstr = 0; + str = PyUnicode_AsUTF8String(str); + if (str) { + char *cstr; + Py_ssize_t len; + if (PyBytes_AsStringAndSize(str, &cstr, &len) != -1) { + newstr = (char *) malloc(len+1); + if (newstr) + memcpy(newstr, cstr, len+1); + } + Py_XDECREF(str); + } + return newstr; +#else + return PyString_AsString(str); +#endif +} + +#if PY_VERSION_HEX >= 0x03030000 || PY_VERSION_HEX < 0x03000000 +# define SWIG_Python_str_DelForPy3(x) +#else +# define SWIG_Python_str_DelForPy3(x) free( (void*) (x) ) +#endif + + +SWIGINTERN PyObject* +SWIG_Python_str_FromChar(const char *c) +{ +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_FromString(c); +#else + return PyString_FromString(c); +#endif +} + +#ifndef PyObject_DEL +# define PyObject_DEL PyObject_Del +#endif + +// SWIGPY_USE_CAPSULE is no longer used within SWIG itself, but some user +// interface files check for it. +# define SWIGPY_USE_CAPSULE +# define SWIGPY_CAPSULE_NAME ("swig_runtime_data" SWIG_RUNTIME_VERSION ".type_pointer_capsule" SWIG_TYPE_TABLE_NAME) + +#if PY_VERSION_HEX < 0x03020000 +#define PyDescr_TYPE(x) (((PyDescrObject *)(x))->d_type) +#define PyDescr_NAME(x) (((PyDescrObject *)(x))->d_name) +#define Py_hash_t long +#endif + +/* ----------------------------------------------------------------------------- + * error manipulation + * ----------------------------------------------------------------------------- */ + +SWIGRUNTIME PyObject* +SWIG_Python_ErrorType(int code) { + PyObject* type = 0; + switch(code) { + case SWIG_MemoryError: + type = PyExc_MemoryError; + break; + case SWIG_IOError: + type = PyExc_IOError; + break; + case SWIG_RuntimeError: + type = PyExc_RuntimeError; + break; + case SWIG_IndexError: + type = PyExc_IndexError; + break; + case SWIG_TypeError: + type = PyExc_TypeError; + break; + case SWIG_DivisionByZero: + type = PyExc_ZeroDivisionError; + break; + case SWIG_OverflowError: + type = PyExc_OverflowError; + break; + case SWIG_SyntaxError: + type = PyExc_SyntaxError; + break; + case SWIG_ValueError: + type = PyExc_ValueError; + break; + case SWIG_SystemError: + type = PyExc_SystemError; + break; + case SWIG_AttributeError: + type = PyExc_AttributeError; + break; + default: + type = PyExc_RuntimeError; + } + return type; +} + + +SWIGRUNTIME void +SWIG_Python_AddErrorMsg(const char* mesg) +{ + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + + if (PyErr_Occurred()) + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + PyErr_Clear(); + Py_XINCREF(type); + if (tmp) + PyErr_Format(type, "%s %s", tmp, mesg); + else + PyErr_Format(type, "%s", mesg); + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + Py_DECREF(value); + } else { + PyErr_SetString(PyExc_RuntimeError, mesg); + } +} + +SWIGRUNTIME int +SWIG_Python_TypeErrorOccurred(PyObject *obj) +{ + PyObject *error; + if (obj) + return 0; + error = PyErr_Occurred(); + return error && PyErr_GivenExceptionMatches(error, PyExc_TypeError); +} + +SWIGRUNTIME void +SWIG_Python_RaiseOrModifyTypeError(const char *message) +{ + if (SWIG_Python_TypeErrorOccurred(NULL)) { + /* Use existing TypeError to preserve stacktrace and enhance with given message */ + PyObject *newvalue; + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); +#if PY_VERSION_HEX >= 0x03000000 + newvalue = PyUnicode_FromFormat("%S\nAdditional information:\n%s", value, message); +#else + newvalue = PyString_FromFormat("%s\nAdditional information:\n%s", PyString_AsString(value), message); +#endif + Py_XDECREF(value); + PyErr_Restore(type, newvalue, traceback); + } else { + /* Raise TypeError using given message */ + PyErr_SetString(PyExc_TypeError, message); + } +} + +#if defined(SWIG_PYTHON_NO_THREADS) +# if defined(SWIG_PYTHON_THREADS) +# undef SWIG_PYTHON_THREADS +# endif +#endif +#if defined(SWIG_PYTHON_THREADS) /* Threading support is enabled */ +# if !defined(SWIG_PYTHON_USE_GIL) && !defined(SWIG_PYTHON_NO_USE_GIL) +# define SWIG_PYTHON_USE_GIL +# endif +# if defined(SWIG_PYTHON_USE_GIL) /* Use PyGILState threads calls */ +# ifndef SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_INITIALIZE_THREADS PyEval_InitThreads() +# endif +# ifdef __cplusplus /* C++ code */ + class SWIG_Python_Thread_Block { + bool status; + PyGILState_STATE state; + public: + void end() { if (status) { PyGILState_Release(state); status = false;} } + SWIG_Python_Thread_Block() : status(true), state(PyGILState_Ensure()) {} + ~SWIG_Python_Thread_Block() { end(); } + }; + class SWIG_Python_Thread_Allow { + bool status; + PyThreadState *save; + public: + void end() { if (status) { PyEval_RestoreThread(save); status = false; }} + SWIG_Python_Thread_Allow() : status(true), save(PyEval_SaveThread()) {} + ~SWIG_Python_Thread_Allow() { end(); } + }; +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK SWIG_Python_Thread_Block _swig_thread_block +# define SWIG_PYTHON_THREAD_END_BLOCK _swig_thread_block.end() +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW SWIG_Python_Thread_Allow _swig_thread_allow +# define SWIG_PYTHON_THREAD_END_ALLOW _swig_thread_allow.end() +# else /* C code */ +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK PyGILState_STATE _swig_thread_block = PyGILState_Ensure() +# define SWIG_PYTHON_THREAD_END_BLOCK PyGILState_Release(_swig_thread_block) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW PyThreadState *_swig_thread_allow = PyEval_SaveThread() +# define SWIG_PYTHON_THREAD_END_ALLOW PyEval_RestoreThread(_swig_thread_allow) +# endif +# else /* Old thread way, not implemented, user must provide it */ +# if !defined(SWIG_PYTHON_INITIALIZE_THREADS) +# define SWIG_PYTHON_INITIALIZE_THREADS +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_BLOCK) +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_END_BLOCK) +# define SWIG_PYTHON_THREAD_END_BLOCK +# endif +# if !defined(SWIG_PYTHON_THREAD_BEGIN_ALLOW) +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# endif +# if !defined(SWIG_PYTHON_THREAD_END_ALLOW) +# define SWIG_PYTHON_THREAD_END_ALLOW +# endif +# endif +#else /* No thread support */ +# define SWIG_PYTHON_INITIALIZE_THREADS +# define SWIG_PYTHON_THREAD_BEGIN_BLOCK +# define SWIG_PYTHON_THREAD_END_BLOCK +# define SWIG_PYTHON_THREAD_BEGIN_ALLOW +# define SWIG_PYTHON_THREAD_END_ALLOW +#endif + +/* ----------------------------------------------------------------------------- + * Python API portion that goes into the runtime + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* ----------------------------------------------------------------------------- + * Constant declarations + * ----------------------------------------------------------------------------- */ + +/* Constant Types */ +#define SWIG_PY_POINTER 4 +#define SWIG_PY_BINARY 5 + +/* Constant information structure */ +typedef struct swig_const_info { + int type; + const char *name; + long lvalue; + double dvalue; + void *pvalue; + swig_type_info **ptype; +} swig_const_info; + +#ifdef __cplusplus +} +#endif + + +/* ----------------------------------------------------------------------------- + * pyrun.swg + * + * This file contains the runtime support for Python modules + * and includes code for managing global variables and pointer + * type checking. + * + * ----------------------------------------------------------------------------- */ + +#if PY_VERSION_HEX < 0x02070000 /* 2.7.0 */ +# error "This version of SWIG only supports Python >= 2.7" +#endif + +#if PY_VERSION_HEX >= 0x03000000 && PY_VERSION_HEX < 0x03020000 +# error "This version of SWIG only supports Python 3 >= 3.2" +#endif + +/* Common SWIG API */ + +/* for raw pointers */ +#define SWIG_Python_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, 0) +#define SWIG_ConvertPtr(obj, pptr, type, flags) SWIG_Python_ConvertPtr(obj, pptr, type, flags) +#define SWIG_ConvertPtrAndOwn(obj,pptr,type,flags,own) SWIG_Python_ConvertPtrAndOwn(obj, pptr, type, flags, own) + +#ifdef SWIGPYTHON_BUILTIN +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(self, ptr, type, flags) +#else +#define SWIG_NewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) +#endif + +#define SWIG_InternalNewPointerObj(ptr, type, flags) SWIG_Python_NewPointerObj(NULL, ptr, type, flags) + +#define SWIG_CheckImplicit(ty) SWIG_Python_CheckImplicit(ty) +#define SWIG_AcquirePtr(ptr, src) SWIG_Python_AcquirePtr(ptr, src) +#define swig_owntype int + +/* for raw packed data */ +#define SWIG_ConvertPacked(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewPackedObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + +/* for class or struct pointers */ +#define SWIG_ConvertInstance(obj, pptr, type, flags) SWIG_ConvertPtr(obj, pptr, type, flags) +#define SWIG_NewInstanceObj(ptr, type, flags) SWIG_NewPointerObj(ptr, type, flags) + +/* for C or C++ function pointers */ +#define SWIG_ConvertFunctionPtr(obj, pptr, type) SWIG_Python_ConvertFunctionPtr(obj, pptr, type) +#define SWIG_NewFunctionPtrObj(ptr, type) SWIG_Python_NewPointerObj(NULL, ptr, type, 0) + +/* for C++ member pointers, ie, member methods */ +#define SWIG_ConvertMember(obj, ptr, sz, ty) SWIG_Python_ConvertPacked(obj, ptr, sz, ty) +#define SWIG_NewMemberObj(ptr, sz, type) SWIG_Python_NewPackedObj(ptr, sz, type) + + +/* Runtime API */ + +#define SWIG_GetModule(clientdata) SWIG_Python_GetModule(clientdata) +#define SWIG_SetModule(clientdata, pointer) SWIG_Python_SetModule(pointer) +#define SWIG_NewClientData(obj) SwigPyClientData_New(obj) + +#define SWIG_SetErrorObj SWIG_Python_SetErrorObj +#define SWIG_SetErrorMsg SWIG_Python_SetErrorMsg +#define SWIG_ErrorType(code) SWIG_Python_ErrorType(code) +#define SWIG_Error(code, msg) SWIG_Python_SetErrorMsg(SWIG_ErrorType(code), msg) +#define SWIG_fail goto fail + + +/* Runtime API implementation */ + +/* Error manipulation */ + +SWIGINTERN void +SWIG_Python_SetErrorObj(PyObject *errtype, PyObject *obj) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetObject(errtype, obj); + Py_DECREF(obj); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +SWIGINTERN void +SWIG_Python_SetErrorMsg(PyObject *errtype, const char *msg) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + PyErr_SetString(errtype, msg); + SWIG_PYTHON_THREAD_END_BLOCK; +} + +#define SWIG_Python_Raise(obj, type, desc) SWIG_Python_SetErrorObj(SWIG_Python_ExceptionType(desc), obj) + +/* Set a constant value */ + +#if defined(SWIGPYTHON_BUILTIN) + +SWIGINTERN void +SwigPyBuiltin_AddPublicSymbol(PyObject *seq, const char *key) { + PyObject *s = PyString_InternFromString(key); + PyList_Append(seq, s); + Py_DECREF(s); +} + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, PyObject *public_interface, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); + if (public_interface) + SwigPyBuiltin_AddPublicSymbol(public_interface, name); +} + +#else + +SWIGINTERN void +SWIG_Python_SetConstant(PyObject *d, const char *name, PyObject *obj) { + PyDict_SetItemString(d, name, obj); + Py_DECREF(obj); +} + +#endif + +/* Append a value to the result obj */ + +SWIGINTERN PyObject* +SWIG_Python_AppendOutput(PyObject* result, PyObject* obj) { + if (!result) { + result = obj; + } else if (result == Py_None) { + Py_DECREF(result); + result = obj; + } else { + if (!PyList_Check(result)) { + PyObject *o2 = result; + result = PyList_New(1); + PyList_SetItem(result, 0, o2); + } + PyList_Append(result,obj); + Py_DECREF(obj); + } + return result; +} + +/* Unpack the argument tuple */ + +SWIGINTERN Py_ssize_t +SWIG_Python_UnpackTuple(PyObject *args, const char *name, Py_ssize_t min, Py_ssize_t max, PyObject **objs) +{ + if (!args) { + if (!min && !max) { + return 1; + } else { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got none", + name, (min == max ? "" : "at least "), (int)min); + return 0; + } + } + if (!PyTuple_Check(args)) { + if (min <= 1 && max >= 1) { + Py_ssize_t i; + objs[0] = args; + for (i = 1; i < max; ++i) { + objs[i] = 0; + } + return 2; + } + PyErr_SetString(PyExc_SystemError, "UnpackTuple() argument list is not a tuple"); + return 0; + } else { + Py_ssize_t l = PyTuple_GET_SIZE(args); + if (l < min) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at least "), (int)min, (int)l); + return 0; + } else if (l > max) { + PyErr_Format(PyExc_TypeError, "%s expected %s%d arguments, got %d", + name, (min == max ? "" : "at most "), (int)max, (int)l); + return 0; + } else { + Py_ssize_t i; + for (i = 0; i < l; ++i) { + objs[i] = PyTuple_GET_ITEM(args, i); + } + for (; l < max; ++l) { + objs[l] = 0; + } + return i + 1; + } + } +} + +SWIGINTERN int +SWIG_Python_CheckNoKeywords(PyObject *kwargs, const char *name) { + int no_kwargs = 1; + if (kwargs) { + assert(PyDict_Check(kwargs)); + if (PyDict_Size(kwargs) > 0) { + PyErr_Format(PyExc_TypeError, "%s() does not take keyword arguments", name); + no_kwargs = 0; + } + } + return no_kwargs; +} + +/* A functor is a function object with one single object argument */ +#define SWIG_Python_CallFunctor(functor, obj) PyObject_CallFunctionObjArgs(functor, obj, NULL); + +/* + Helper for static pointer initialization for both C and C++ code, for example + static PyObject *SWIG_STATIC_POINTER(MyVar) = NewSomething(...); +*/ +#ifdef __cplusplus +#define SWIG_STATIC_POINTER(var) var +#else +#define SWIG_STATIC_POINTER(var) var = 0; if (!var) var +#endif + +/* ----------------------------------------------------------------------------- + * Pointer declarations + * ----------------------------------------------------------------------------- */ + +/* Flags for new pointer objects */ +#define SWIG_POINTER_NOSHADOW (SWIG_POINTER_OWN << 1) +#define SWIG_POINTER_NEW (SWIG_POINTER_NOSHADOW | SWIG_POINTER_OWN) + +#define SWIG_POINTER_IMPLICIT_CONV (SWIG_POINTER_DISOWN << 1) + +#define SWIG_BUILTIN_TP_INIT (SWIG_POINTER_OWN << 2) +#define SWIG_BUILTIN_INIT (SWIG_BUILTIN_TP_INIT | SWIG_POINTER_OWN) + +#ifdef __cplusplus +extern "C" { +#endif + +/* The python void return value */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Py_Void(void) +{ + PyObject *none = Py_None; + Py_INCREF(none); + return none; +} + +/* SwigPyClientData */ + +typedef struct { + PyObject *klass; + PyObject *newraw; + PyObject *newargs; + PyObject *destroy; + int delargs; + int implicitconv; + PyTypeObject *pytype; +} SwigPyClientData; + +SWIGRUNTIMEINLINE int +SWIG_Python_CheckImplicit(swig_type_info *ty) +{ + SwigPyClientData *data = (SwigPyClientData *)ty->clientdata; + int fail = data ? data->implicitconv : 0; + if (fail) + PyErr_SetString(PyExc_TypeError, "Implicit conversion is prohibited for explicit constructors."); + return fail; +} + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_ExceptionType(swig_type_info *desc) { + SwigPyClientData *data = desc ? (SwigPyClientData *) desc->clientdata : 0; + PyObject *klass = data ? data->klass : 0; + return (klass ? klass : PyExc_RuntimeError); +} + + +SWIGRUNTIME SwigPyClientData * +SwigPyClientData_New(PyObject* obj) +{ + if (!obj) { + return 0; + } else { + SwigPyClientData *data = (SwigPyClientData *)malloc(sizeof(SwigPyClientData)); + /* the klass element */ + data->klass = obj; + Py_INCREF(data->klass); + /* the newraw method and newargs arguments used to create a new raw instance */ + if (PyClass_Check(obj)) { + data->newraw = 0; + data->newargs = obj; + Py_INCREF(obj); + } else { + data->newraw = PyObject_GetAttrString(data->klass, "__new__"); + if (data->newraw) { + Py_INCREF(data->newraw); + data->newargs = PyTuple_New(1); + PyTuple_SetItem(data->newargs, 0, obj); + } else { + data->newargs = obj; + } + Py_INCREF(data->newargs); + } + /* the destroy method, aka as the C++ delete method */ + data->destroy = PyObject_GetAttrString(data->klass, "__swig_destroy__"); + if (PyErr_Occurred()) { + PyErr_Clear(); + data->destroy = 0; + } + if (data->destroy) { + int flags; + Py_INCREF(data->destroy); + flags = PyCFunction_GET_FLAGS(data->destroy); + data->delargs = !(flags & (METH_O)); + } else { + data->delargs = 0; + } + data->implicitconv = 0; + data->pytype = 0; + return data; + } +} + +SWIGRUNTIME void +SwigPyClientData_Del(SwigPyClientData *data) { + Py_XDECREF(data->newraw); + Py_XDECREF(data->newargs); + Py_XDECREF(data->destroy); +} + +/* =============== SwigPyObject =====================*/ + +typedef struct { + PyObject_HEAD + void *ptr; + swig_type_info *ty; + int own; + PyObject *next; +#ifdef SWIGPYTHON_BUILTIN + PyObject *dict; +#endif +} SwigPyObject; + + +#ifdef SWIGPYTHON_BUILTIN + +SWIGRUNTIME PyObject * +SwigPyObject_get___dict__(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + + if (!sobj->dict) + sobj->dict = PyDict_New(); + + Py_INCREF(sobj->dict); + return sobj->dict; +} + +#endif + +SWIGRUNTIME PyObject * +SwigPyObject_long(SwigPyObject *v) +{ + return PyLong_FromVoidPtr(v->ptr); +} + +SWIGRUNTIME PyObject * +SwigPyObject_format(const char* fmt, SwigPyObject *v) +{ + PyObject *res = NULL; + PyObject *args = PyTuple_New(1); + if (args) { + if (PyTuple_SetItem(args, 0, SwigPyObject_long(v)) == 0) { + PyObject *ofmt = SWIG_Python_str_FromChar(fmt); + if (ofmt) { +#if PY_VERSION_HEX >= 0x03000000 + res = PyUnicode_Format(ofmt,args); +#else + res = PyString_Format(ofmt,args); +#endif + Py_DECREF(ofmt); + } + Py_DECREF(args); + } + } + return res; +} + +SWIGRUNTIME PyObject * +SwigPyObject_oct(SwigPyObject *v) +{ + return SwigPyObject_format("%o",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_hex(SwigPyObject *v) +{ + return SwigPyObject_format("%x",v); +} + +SWIGRUNTIME PyObject * +SwigPyObject_repr(SwigPyObject *v) +{ + const char *name = SWIG_TypePrettyName(v->ty); + PyObject *repr = SWIG_Python_str_FromFormat("", (name ? name : "unknown"), (void *)v); + if (v->next) { + PyObject *nrep = SwigPyObject_repr((SwigPyObject *)v->next); +# if PY_VERSION_HEX >= 0x03000000 + PyObject *joined = PyUnicode_Concat(repr, nrep); + Py_DecRef(repr); + Py_DecRef(nrep); + repr = joined; +# else + PyString_ConcatAndDel(&repr,nrep); +# endif + } + return repr; +} + +/* We need a version taking two PyObject* parameters so it's a valid + * PyCFunction to use in swigobject_methods[]. */ +SWIGRUNTIME PyObject * +SwigPyObject_repr2(PyObject *v, PyObject *SWIGUNUSEDPARM(args)) +{ + return SwigPyObject_repr((SwigPyObject*)v); +} + +SWIGRUNTIME int +SwigPyObject_compare(SwigPyObject *v, SwigPyObject *w) +{ + void *i = v->ptr; + void *j = w->ptr; + return (i < j) ? -1 : ((i > j) ? 1 : 0); +} + +/* Added for Python 3.x, would it also be useful for Python 2.x? */ +SWIGRUNTIME PyObject* +SwigPyObject_richcompare(SwigPyObject *v, SwigPyObject *w, int op) +{ + PyObject* res; + if( op != Py_EQ && op != Py_NE ) { + Py_INCREF(Py_NotImplemented); + return Py_NotImplemented; + } + res = PyBool_FromLong( (SwigPyObject_compare(v, w)==0) == (op == Py_EQ) ? 1 : 0); + return res; +} + + +SWIGRUNTIME PyTypeObject* SwigPyObject_TypeOnce(void); + +#ifdef SWIGPYTHON_BUILTIN +static swig_type_info *SwigPyObject_stype = 0; +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + SwigPyClientData *cd; + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + assert(cd); + assert(cd->pytype); + return cd->pytype; +} +#else +SWIGRUNTIME PyTypeObject* +SwigPyObject_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyObject_TypeOnce(); + return type; +} +#endif + +SWIGRUNTIMEINLINE int +SwigPyObject_Check(PyObject *op) { +#ifdef SWIGPYTHON_BUILTIN + PyTypeObject *target_tp = SwigPyObject_type(); + if (PyType_IsSubtype(op->ob_type, target_tp)) + return 1; + return (strcmp(op->ob_type->tp_name, "SwigPyObject") == 0); +#else + return (Py_TYPE(op) == SwigPyObject_type()) + || (strcmp(Py_TYPE(op)->tp_name,"SwigPyObject") == 0); +#endif +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own); + +SWIGRUNTIME void +SwigPyObject_dealloc(PyObject *v) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + PyObject *next = sobj->next; + if (sobj->own == SWIG_POINTER_OWN) { + swig_type_info *ty = sobj->ty; + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + PyObject *destroy = data ? data->destroy : 0; + if (destroy) { + /* destroy is always a VARARGS method */ + PyObject *res; + + /* PyObject_CallFunction() has the potential to silently drop + the active exception. In cases of unnamed temporary + variable or where we just finished iterating over a generator + StopIteration will be active right now, and this needs to + remain true upon return from SwigPyObject_dealloc. So save + and restore. */ + + PyObject *type = NULL, *value = NULL, *traceback = NULL; + PyErr_Fetch(&type, &value, &traceback); + + if (data->delargs) { + /* we need to create a temporary object to carry the destroy operation */ + PyObject *tmp = SwigPyObject_New(sobj->ptr, ty, 0); + res = SWIG_Python_CallFunctor(destroy, tmp); + Py_DECREF(tmp); + } else { + PyCFunction meth = PyCFunction_GET_FUNCTION(destroy); + PyObject *mself = PyCFunction_GET_SELF(destroy); + res = ((*meth)(mself, v)); + } + if (!res) + PyErr_WriteUnraisable(destroy); + + PyErr_Restore(type, value, traceback); + + Py_XDECREF(res); + } +#if !defined(SWIG_PYTHON_SILENT_MEMLEAK) + else { + const char *name = SWIG_TypePrettyName(ty); + printf("swig/python detected a memory leak of type '%s', no destructor found.\n", (name ? name : "unknown")); + } +#endif + } + Py_XDECREF(next); + PyObject_DEL(v); +} + +SWIGRUNTIME PyObject* +SwigPyObject_append(PyObject* v, PyObject* next) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (!SwigPyObject_Check(next)) { + PyErr_SetString(PyExc_TypeError, "Attempt to append a non SwigPyObject"); + return NULL; + } + sobj->next = next; + Py_INCREF(next); + return SWIG_Py_Void(); +} + +SWIGRUNTIME PyObject* +SwigPyObject_next(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *) v; + if (sobj->next) { + Py_INCREF(sobj->next); + return sobj->next; + } else { + return SWIG_Py_Void(); + } +} + +SWIGINTERN PyObject* +SwigPyObject_disown(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = 0; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_acquire(PyObject* v, PyObject *SWIGUNUSEDPARM(args)) +{ + SwigPyObject *sobj = (SwigPyObject *)v; + sobj->own = SWIG_POINTER_OWN; + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject* +SwigPyObject_own(PyObject *v, PyObject *args) +{ + PyObject *val = 0; + if (!PyArg_UnpackTuple(args, "own", 0, 1, &val)) { + return NULL; + } else { + SwigPyObject *sobj = (SwigPyObject *)v; + PyObject *obj = PyBool_FromLong(sobj->own); + if (val) { + if (PyObject_IsTrue(val)) { + SwigPyObject_acquire(v,args); + } else { + SwigPyObject_disown(v,args); + } + } + return obj; + } +} + +static PyMethodDef +swigobject_methods[] = { + {"disown", SwigPyObject_disown, METH_NOARGS, "releases ownership of the pointer"}, + {"acquire", SwigPyObject_acquire, METH_NOARGS, "acquires ownership of the pointer"}, + {"own", SwigPyObject_own, METH_VARARGS, "returns/sets ownership of the pointer"}, + {"append", SwigPyObject_append, METH_O, "appends another 'this' object"}, + {"next", SwigPyObject_next, METH_NOARGS, "returns the next 'this' object"}, + {"__repr__",SwigPyObject_repr2, METH_NOARGS, "returns object representation"}, + {0, 0, 0, 0} +}; + +SWIGRUNTIME PyTypeObject* +SwigPyObject_TypeOnce(void) { + static char swigobject_doc[] = "Swig object carries a C/C++ instance pointer"; + + static PyNumberMethods SwigPyObject_as_number = { + (binaryfunc)0, /*nb_add*/ + (binaryfunc)0, /*nb_subtract*/ + (binaryfunc)0, /*nb_multiply*/ + /* nb_divide removed in Python 3 */ +#if PY_VERSION_HEX < 0x03000000 + (binaryfunc)0, /*nb_divide*/ +#endif + (binaryfunc)0, /*nb_remainder*/ + (binaryfunc)0, /*nb_divmod*/ + (ternaryfunc)0,/*nb_power*/ + (unaryfunc)0, /*nb_negative*/ + (unaryfunc)0, /*nb_positive*/ + (unaryfunc)0, /*nb_absolute*/ + (inquiry)0, /*nb_nonzero*/ + 0, /*nb_invert*/ + 0, /*nb_lshift*/ + 0, /*nb_rshift*/ + 0, /*nb_and*/ + 0, /*nb_xor*/ + 0, /*nb_or*/ +#if PY_VERSION_HEX < 0x03000000 + 0, /*nb_coerce*/ +#endif + (unaryfunc)SwigPyObject_long, /*nb_int*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_long, /*nb_long*/ +#else + 0, /*nb_reserved*/ +#endif + (unaryfunc)0, /*nb_float*/ +#if PY_VERSION_HEX < 0x03000000 + (unaryfunc)SwigPyObject_oct, /*nb_oct*/ + (unaryfunc)SwigPyObject_hex, /*nb_hex*/ +#endif +#if PY_VERSION_HEX >= 0x03050000 /* 3.5 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_inplace_matrix_multiply */ +#elif PY_VERSION_HEX >= 0x03000000 /* 3.0 */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index, nb_inplace_divide removed */ +#else + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0 /* nb_inplace_add -> nb_index */ +#endif + }; + + static PyTypeObject swigpyobject_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyObject", /* tp_name */ + sizeof(SwigPyObject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyObject_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX >= 0x03000000 + 0, /* tp_reserved in 3.0.1, tp_compare in 3.0.0 but not used */ +#else + (cmpfunc)SwigPyObject_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyObject_repr, /* tp_repr */ + &SwigPyObject_as_number, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + 0, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigobject_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + (richcmpfunc)SwigPyObject_richcompare,/* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + swigobject_methods, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpyobject_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpyobject_type) < 0) + return NULL; + } + return &swigpyobject_type; +} + +SWIGRUNTIME PyObject * +SwigPyObject_New(void *ptr, swig_type_info *ty, int own) +{ + SwigPyObject *sobj = PyObject_NEW(SwigPyObject, SwigPyObject_type()); + if (sobj) { + sobj->ptr = ptr; + sobj->ty = ty; + sobj->own = own; + sobj->next = 0; + } + return (PyObject *)sobj; +} + +/* ----------------------------------------------------------------------------- + * Implements a simple Swig Packed type, and use it instead of string + * ----------------------------------------------------------------------------- */ + +typedef struct { + PyObject_HEAD + void *pack; + swig_type_info *ty; + size_t size; +} SwigPyPacked; + +SWIGRUNTIME PyObject * +SwigPyPacked_repr(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))) { + return SWIG_Python_str_FromFormat("", result, v->ty->name); + } else { + return SWIG_Python_str_FromFormat("", v->ty->name); + } +} + +SWIGRUNTIME PyObject * +SwigPyPacked_str(SwigPyPacked *v) +{ + char result[SWIG_BUFFER_SIZE]; + if (SWIG_PackDataName(result, v->pack, v->size, 0, sizeof(result))){ + return SWIG_Python_str_FromFormat("%s%s", result, v->ty->name); + } else { + return SWIG_Python_str_FromChar(v->ty->name); + } +} + +SWIGRUNTIME int +SwigPyPacked_compare(SwigPyPacked *v, SwigPyPacked *w) +{ + size_t i = v->size; + size_t j = w->size; + int s = (i < j) ? -1 : ((i > j) ? 1 : 0); + return s ? s : strncmp((const char *)v->pack, (const char *)w->pack, 2*v->size); +} + +SWIGRUNTIME PyTypeObject* SwigPyPacked_TypeOnce(void); + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_type(void) { + static PyTypeObject *SWIG_STATIC_POINTER(type) = SwigPyPacked_TypeOnce(); + return type; +} + +SWIGRUNTIMEINLINE int +SwigPyPacked_Check(PyObject *op) { + return ((op)->ob_type == SwigPyPacked_TypeOnce()) + || (strcmp((op)->ob_type->tp_name,"SwigPyPacked") == 0); +} + +SWIGRUNTIME void +SwigPyPacked_dealloc(PyObject *v) +{ + if (SwigPyPacked_Check(v)) { + SwigPyPacked *sobj = (SwigPyPacked *) v; + free(sobj->pack); + } + PyObject_DEL(v); +} + +SWIGRUNTIME PyTypeObject* +SwigPyPacked_TypeOnce(void) { + static char swigpacked_doc[] = "Swig object carries a C/C++ instance pointer"; + static PyTypeObject swigpypacked_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX>=0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "SwigPyPacked", /* tp_name */ + sizeof(SwigPyPacked), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor)SwigPyPacked_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc)0, /* tp_getattr */ + (setattrfunc)0, /* tp_setattr */ +#if PY_VERSION_HEX>=0x03000000 + 0, /* tp_reserved in 3.0.1 */ +#else + (cmpfunc)SwigPyPacked_compare, /* tp_compare */ +#endif + (reprfunc)SwigPyPacked_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + (hashfunc)0, /* tp_hash */ + (ternaryfunc)0, /* tp_call */ + (reprfunc)SwigPyPacked_str, /* tp_str */ + PyObject_GenericGetAttr, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + Py_TPFLAGS_DEFAULT, /* tp_flags */ + swigpacked_doc, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0, /* tp_iter */ + 0, /* tp_iternext */ + 0, /* tp_methods */ + 0, /* tp_members */ + 0, /* tp_getset */ + 0, /* tp_base */ + 0, /* tp_dict */ + 0, /* tp_descr_get */ + 0, /* tp_descr_set */ + 0, /* tp_dictoffset */ + 0, /* tp_init */ + 0, /* tp_alloc */ + 0, /* tp_new */ + 0, /* tp_free */ + 0, /* tp_is_gc */ + 0, /* tp_bases */ + 0, /* tp_mro */ + 0, /* tp_cache */ + 0, /* tp_subclasses */ + 0, /* tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + swigpypacked_type = tmp; + type_init = 1; + if (PyType_Ready(&swigpypacked_type) < 0) + return NULL; + } + return &swigpypacked_type; +} + +SWIGRUNTIME PyObject * +SwigPyPacked_New(void *ptr, size_t size, swig_type_info *ty) +{ + SwigPyPacked *sobj = PyObject_NEW(SwigPyPacked, SwigPyPacked_type()); + if (sobj) { + void *pack = malloc(size); + if (pack) { + memcpy(pack, ptr, size); + sobj->pack = pack; + sobj->ty = ty; + sobj->size = size; + } else { + PyObject_DEL((PyObject *) sobj); + sobj = 0; + } + } + return (PyObject *) sobj; +} + +SWIGRUNTIME swig_type_info * +SwigPyPacked_UnpackData(PyObject *obj, void *ptr, size_t size) +{ + if (SwigPyPacked_Check(obj)) { + SwigPyPacked *sobj = (SwigPyPacked *)obj; + if (sobj->size != size) return 0; + memcpy(ptr, sobj->pack, size); + return sobj->ty; + } else { + return 0; + } +} + +/* ----------------------------------------------------------------------------- + * pointers/data manipulation + * ----------------------------------------------------------------------------- */ + +static PyObject *Swig_This_global = NULL; + +SWIGRUNTIME PyObject * +SWIG_This(void) +{ + if (Swig_This_global == NULL) + Swig_This_global = SWIG_Python_str_FromChar("this"); + return Swig_This_global; +} + +/* #define SWIG_PYTHON_SLOW_GETSET_THIS */ + +/* TODO: I don't know how to implement the fast getset in Python 3 right now */ +#if PY_VERSION_HEX>=0x03000000 +#define SWIG_PYTHON_SLOW_GETSET_THIS +#endif + +SWIGRUNTIME SwigPyObject * +SWIG_Python_GetSwigThis(PyObject *pyobj) +{ + PyObject *obj; + + if (SwigPyObject_Check(pyobj)) + return (SwigPyObject *) pyobj; + +#ifdef SWIGPYTHON_BUILTIN + (void)obj; +# ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + pyobj = PyWeakref_GET_OBJECT(pyobj); + if (pyobj && SwigPyObject_Check(pyobj)) + return (SwigPyObject*) pyobj; + } +# endif + return NULL; +#else + + obj = 0; + +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + if (PyInstance_Check(pyobj)) { + obj = _PyInstance_Lookup(pyobj, SWIG_This()); + } else { + PyObject **dictptr = _PyObject_GetDictPtr(pyobj); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + obj = dict ? PyDict_GetItem(dict, SWIG_This()) : 0; + } else { +#ifdef PyWeakref_CheckProxy + if (PyWeakref_CheckProxy(pyobj)) { + PyObject *wobj = PyWeakref_GET_OBJECT(pyobj); + return wobj ? SWIG_Python_GetSwigThis(wobj) : 0; + } +#endif + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } + } + } +#else + obj = PyObject_GetAttr(pyobj,SWIG_This()); + if (obj) { + Py_DECREF(obj); + } else { + if (PyErr_Occurred()) PyErr_Clear(); + return 0; + } +#endif + if (obj && !SwigPyObject_Check(obj)) { + /* a PyObject is called 'this', try to get the 'real this' + SwigPyObject from it */ + return SWIG_Python_GetSwigThis(obj); + } + return (SwigPyObject *)obj; +#endif +} + +/* Acquire a pointer value */ + +SWIGRUNTIME int +SWIG_Python_AcquirePtr(PyObject *obj, int own) { + if (own == SWIG_POINTER_OWN) { + SwigPyObject *sobj = SWIG_Python_GetSwigThis(obj); + if (sobj) { + int oldown = sobj->own; + sobj->own = own; + return oldown; + } + } + return 0; +} + +/* Convert a pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPtrAndOwn(PyObject *obj, void **ptr, swig_type_info *ty, int flags, int *own) { + int res; + SwigPyObject *sobj; + int implicit_conv = (flags & SWIG_POINTER_IMPLICIT_CONV) != 0; + + if (!obj) + return SWIG_ERROR; + if (obj == Py_None && !implicit_conv) { + if (ptr) + *ptr = 0; + return (flags & SWIG_POINTER_NO_NULL) ? SWIG_NullReferenceError : SWIG_OK; + } + + res = SWIG_ERROR; + + sobj = SWIG_Python_GetSwigThis(obj); + if (own) + *own = 0; + while (sobj) { + void *vptr = sobj->ptr; + if (ty) { + swig_type_info *to = sobj->ty; + if (to == ty) { + /* no type cast needed */ + if (ptr) *ptr = vptr; + break; + } else { + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) { + sobj = (SwigPyObject *)sobj->next; + } else { + if (ptr) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + if (newmemory == SWIG_CAST_NEW_MEMORY) { + assert(own); /* badly formed typemap which will lead to a memory leak - it must set and use own to delete *ptr */ + if (own) + *own = *own | SWIG_CAST_NEW_MEMORY; + } + } + break; + } + } + } else { + if (ptr) *ptr = vptr; + break; + } + } + if (sobj) { + if (own) + *own = *own | sobj->own; + if (flags & SWIG_POINTER_DISOWN) { + sobj->own = 0; + } + res = SWIG_OK; + } else { + if (implicit_conv) { + SwigPyClientData *data = ty ? (SwigPyClientData *) ty->clientdata : 0; + if (data && !data->implicitconv) { + PyObject *klass = data->klass; + if (klass) { + PyObject *impconv; + data->implicitconv = 1; /* avoid recursion and call 'explicit' constructors*/ + impconv = SWIG_Python_CallFunctor(klass, obj); + data->implicitconv = 0; + if (PyErr_Occurred()) { + PyErr_Clear(); + impconv = 0; + } + if (impconv) { + SwigPyObject *iobj = SWIG_Python_GetSwigThis(impconv); + if (iobj) { + void *vptr; + res = SWIG_Python_ConvertPtrAndOwn((PyObject*)iobj, &vptr, ty, 0, 0); + if (SWIG_IsOK(res)) { + if (ptr) { + *ptr = vptr; + /* transfer the ownership to 'ptr' */ + iobj->own = 0; + res = SWIG_AddCast(res); + res = SWIG_AddNewMask(res); + } else { + res = SWIG_AddCast(res); + } + } + } + Py_DECREF(impconv); + } + } + } + if (!SWIG_IsOK(res) && obj == Py_None) { + if (ptr) + *ptr = 0; + if (PyErr_Occurred()) + PyErr_Clear(); + res = SWIG_OK; + } + } + } + return res; +} + +/* Convert a function ptr value */ + +SWIGRUNTIME int +SWIG_Python_ConvertFunctionPtr(PyObject *obj, void **ptr, swig_type_info *ty) { + if (!PyCFunction_Check(obj)) { + return SWIG_ConvertPtr(obj, ptr, ty, 0); + } else { + void *vptr = 0; + swig_cast_info *tc; + + /* here we get the method pointer for callbacks */ + const char *doc = (((PyCFunctionObject *)obj) -> m_ml -> ml_doc); + const char *desc = doc ? strstr(doc, "swig_ptr: ") : 0; + if (desc) + desc = ty ? SWIG_UnpackVoidPtr(desc + 10, &vptr, ty->name) : 0; + if (!desc) + return SWIG_ERROR; + tc = SWIG_TypeCheck(desc,ty); + if (tc) { + int newmemory = 0; + *ptr = SWIG_TypeCast(tc,vptr,&newmemory); + assert(!newmemory); /* newmemory handling not yet implemented */ + } else { + return SWIG_ERROR; + } + return SWIG_OK; + } +} + +/* Convert a packed pointer value */ + +SWIGRUNTIME int +SWIG_Python_ConvertPacked(PyObject *obj, void *ptr, size_t sz, swig_type_info *ty) { + swig_type_info *to = SwigPyPacked_UnpackData(obj, ptr, sz); + if (!to) return SWIG_ERROR; + if (ty) { + if (to != ty) { + /* check type cast? */ + swig_cast_info *tc = SWIG_TypeCheck(to->name,ty); + if (!tc) return SWIG_ERROR; + } + } + return SWIG_OK; +} + +/* ----------------------------------------------------------------------------- + * Create a new pointer object + * ----------------------------------------------------------------------------- */ + +/* + Create a new instance object, without calling __init__, and set the + 'this' attribute. +*/ + +SWIGRUNTIME PyObject* +SWIG_Python_NewShadowInstance(SwigPyClientData *data, PyObject *swig_this) +{ + PyObject *inst = 0; + PyObject *newraw = data->newraw; + if (newraw) { + inst = PyObject_Call(newraw, data->newargs, NULL); + if (inst) { +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + PyDict_SetItem(dict, SWIG_This(), swig_this); + } + } +#else + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } +#endif + } + } else { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *empty_args = PyTuple_New(0); + if (empty_args) { + PyObject *empty_kwargs = PyDict_New(); + if (empty_kwargs) { + inst = ((PyTypeObject *)data->newargs)->tp_new((PyTypeObject *)data->newargs, empty_args, empty_kwargs); + Py_DECREF(empty_kwargs); + if (inst) { + if (PyObject_SetAttr(inst, SWIG_This(), swig_this) == -1) { + Py_DECREF(inst); + inst = 0; + } else { + Py_TYPE(inst)->tp_flags &= ~Py_TPFLAGS_VALID_VERSION_TAG; + } + } + } + Py_DECREF(empty_args); + } +#else + PyObject *dict = PyDict_New(); + if (dict) { + PyDict_SetItem(dict, SWIG_This(), swig_this); + inst = PyInstance_NewRaw(data->newargs, dict); + Py_DECREF(dict); + } +#endif + } + return inst; +} + +SWIGRUNTIME int +SWIG_Python_SetSwigThis(PyObject *inst, PyObject *swig_this) +{ +#if !defined(SWIG_PYTHON_SLOW_GETSET_THIS) + PyObject **dictptr = _PyObject_GetDictPtr(inst); + if (dictptr != NULL) { + PyObject *dict = *dictptr; + if (dict == NULL) { + dict = PyDict_New(); + *dictptr = dict; + } + return PyDict_SetItem(dict, SWIG_This(), swig_this); + } +#endif + return PyObject_SetAttr(inst, SWIG_This(), swig_this); +} + + +SWIGINTERN PyObject * +SWIG_Python_InitShadowInstance(PyObject *args) { + PyObject *obj[2]; + if (!SWIG_Python_UnpackTuple(args, "swiginit", 2, 2, obj)) { + return NULL; + } else { + SwigPyObject *sthis = SWIG_Python_GetSwigThis(obj[0]); + if (sthis) { + SwigPyObject_append((PyObject*) sthis, obj[1]); + } else { + if (SWIG_Python_SetSwigThis(obj[0], obj[1]) != 0) + return NULL; + } + return SWIG_Py_Void(); + } +} + +/* Create a new pointer object */ + +SWIGRUNTIME PyObject * +SWIG_Python_NewPointerObj(PyObject *self, void *ptr, swig_type_info *type, int flags) { + SwigPyClientData *clientdata; + PyObject * robj; + int own; + + if (!ptr) + return SWIG_Py_Void(); + + clientdata = type ? (SwigPyClientData *)(type->clientdata) : 0; + own = (flags & SWIG_POINTER_OWN) ? SWIG_POINTER_OWN : 0; + if (clientdata && clientdata->pytype) { + SwigPyObject *newobj; + if (flags & SWIG_BUILTIN_TP_INIT) { + newobj = (SwigPyObject*) self; + if (newobj->ptr) { + PyObject *next_self = clientdata->pytype->tp_alloc(clientdata->pytype, 0); + while (newobj->next) + newobj = (SwigPyObject *) newobj->next; + newobj->next = next_self; + newobj = (SwigPyObject *)next_self; +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + } else { + newobj = PyObject_New(SwigPyObject, clientdata->pytype); +#ifdef SWIGPYTHON_BUILTIN + newobj->dict = 0; +#endif + } + if (newobj) { + newobj->ptr = ptr; + newobj->ty = type; + newobj->own = own; + newobj->next = 0; + return (PyObject*) newobj; + } + return SWIG_Py_Void(); + } + + assert(!(flags & SWIG_BUILTIN_TP_INIT)); + + robj = SwigPyObject_New(ptr, type, own); + if (robj && clientdata && !(flags & SWIG_POINTER_NOSHADOW)) { + PyObject *inst = SWIG_Python_NewShadowInstance(clientdata, robj); + Py_DECREF(robj); + robj = inst; + } + return robj; +} + +/* Create a new packed object */ + +SWIGRUNTIMEINLINE PyObject * +SWIG_Python_NewPackedObj(void *ptr, size_t sz, swig_type_info *type) { + return ptr ? SwigPyPacked_New((void *) ptr, sz, type) : SWIG_Py_Void(); +} + +/* -----------------------------------------------------------------------------* + * Get type list + * -----------------------------------------------------------------------------*/ + +#ifdef SWIG_LINK_RUNTIME +void *SWIG_ReturnGlobalTypeList(void *); +#endif + +SWIGRUNTIME swig_module_info * +SWIG_Python_GetModule(void *SWIGUNUSEDPARM(clientdata)) { + static void *type_pointer = (void *)0; + /* first check if module already created */ + if (!type_pointer) { +#ifdef SWIG_LINK_RUNTIME + type_pointer = SWIG_ReturnGlobalTypeList((void *)0); +#else + type_pointer = PyCapsule_Import(SWIGPY_CAPSULE_NAME, 0); + if (PyErr_Occurred()) { + PyErr_Clear(); + type_pointer = (void *)0; + } +#endif + } + return (swig_module_info *) type_pointer; +} + +SWIGRUNTIME void +SWIG_Python_DestroyModule(PyObject *obj) +{ + swig_module_info *swig_module = (swig_module_info *) PyCapsule_GetPointer(obj, SWIGPY_CAPSULE_NAME); + swig_type_info **types = swig_module->types; + size_t i; + for (i =0; i < swig_module->size; ++i) { + swig_type_info *ty = types[i]; + if (ty->owndata) { + SwigPyClientData *data = (SwigPyClientData *) ty->clientdata; + if (data) SwigPyClientData_Del(data); + } + } + Py_DECREF(SWIG_This()); + Swig_This_global = NULL; +} + +SWIGRUNTIME void +SWIG_Python_SetModule(swig_module_info *swig_module) { +#if PY_VERSION_HEX >= 0x03000000 + /* Add a dummy module object into sys.modules */ + PyObject *module = PyImport_AddModule("swig_runtime_data" SWIG_RUNTIME_VERSION); +#else + static PyMethodDef swig_empty_runtime_method_table[] = { {NULL, NULL, 0, NULL} }; /* Sentinel */ + PyObject *module = Py_InitModule("swig_runtime_data" SWIG_RUNTIME_VERSION, swig_empty_runtime_method_table); +#endif + PyObject *pointer = PyCapsule_New((void *) swig_module, SWIGPY_CAPSULE_NAME, SWIG_Python_DestroyModule); + if (pointer && module) { + PyModule_AddObject(module, "type_pointer_capsule" SWIG_TYPE_TABLE_NAME, pointer); + } else { + Py_XDECREF(pointer); + } +} + +/* The python cached type query */ +SWIGRUNTIME PyObject * +SWIG_Python_TypeCache(void) { + static PyObject *SWIG_STATIC_POINTER(cache) = PyDict_New(); + return cache; +} + +SWIGRUNTIME swig_type_info * +SWIG_Python_TypeQuery(const char *type) +{ + PyObject *cache = SWIG_Python_TypeCache(); + PyObject *key = SWIG_Python_str_FromChar(type); + PyObject *obj = PyDict_GetItem(cache, key); + swig_type_info *descriptor; + if (obj) { + descriptor = (swig_type_info *) PyCapsule_GetPointer(obj, NULL); + } else { + swig_module_info *swig_module = SWIG_GetModule(0); + descriptor = SWIG_TypeQueryModule(swig_module, swig_module, type); + if (descriptor) { + obj = PyCapsule_New((void*) descriptor, NULL, NULL); + PyDict_SetItem(cache, key, obj); + Py_DECREF(obj); + } + } + Py_DECREF(key); + return descriptor; +} + +/* + For backward compatibility only +*/ +#define SWIG_POINTER_EXCEPTION 0 +#define SWIG_arg_fail(arg) SWIG_Python_ArgFail(arg) +#define SWIG_MustGetPtr(p, type, argnum, flags) SWIG_Python_MustGetPtr(p, type, argnum, flags) + +SWIGRUNTIME int +SWIG_Python_AddErrMesg(const char* mesg, int infront) +{ + if (PyErr_Occurred()) { + PyObject *type = 0; + PyObject *value = 0; + PyObject *traceback = 0; + PyErr_Fetch(&type, &value, &traceback); + if (value) { + PyObject *old_str = PyObject_Str(value); + const char *tmp = SWIG_Python_str_AsChar(old_str); + const char *errmesg = tmp ? tmp : "Invalid error message"; + Py_XINCREF(type); + PyErr_Clear(); + if (infront) { + PyErr_Format(type, "%s %s", mesg, errmesg); + } else { + PyErr_Format(type, "%s %s", errmesg, mesg); + } + SWIG_Python_str_DelForPy3(tmp); + Py_DECREF(old_str); + } + return 1; + } else { + return 0; + } +} + +SWIGRUNTIME int +SWIG_Python_ArgFail(int argnum) +{ + if (PyErr_Occurred()) { + /* add information about failing argument */ + char mesg[256]; + PyOS_snprintf(mesg, sizeof(mesg), "argument number %d:", argnum); + return SWIG_Python_AddErrMesg(mesg, 1); + } else { + return 0; + } +} + +SWIGRUNTIMEINLINE const char * +SwigPyObject_GetDesc(PyObject *self) +{ + SwigPyObject *v = (SwigPyObject *)self; + swig_type_info *ty = v ? v->ty : 0; + return ty ? ty->str : ""; +} + +SWIGRUNTIME void +SWIG_Python_TypeError(const char *type, PyObject *obj) +{ + if (type) { +#if defined(SWIG_COBJECT_TYPES) + if (obj && SwigPyObject_Check(obj)) { + const char *otype = (const char *) SwigPyObject_GetDesc(obj); + if (otype) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, 'SwigPyObject(%s)' is received", + type, otype); + return; + } + } else +#endif + { + const char *otype = (obj ? obj->ob_type->tp_name : 0); + if (otype) { + PyObject *str = PyObject_Str(obj); + const char *cstr = str ? SWIG_Python_str_AsChar(str) : 0; + if (cstr) { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s(%s)' is received", + type, otype, cstr); + SWIG_Python_str_DelForPy3(cstr); + } else { + PyErr_Format(PyExc_TypeError, "a '%s' is expected, '%s' is received", + type, otype); + } + Py_XDECREF(str); + return; + } + } + PyErr_Format(PyExc_TypeError, "a '%s' is expected", type); + } else { + PyErr_Format(PyExc_TypeError, "unexpected type is received"); + } +} + + +/* Convert a pointer value, signal an exception on a type mismatch */ +SWIGRUNTIME void * +SWIG_Python_MustGetPtr(PyObject *obj, swig_type_info *ty, int SWIGUNUSEDPARM(argnum), int flags) { + void *result; + if (SWIG_Python_ConvertPtr(obj, &result, ty, flags) == -1) { + PyErr_Clear(); +#if SWIG_POINTER_EXCEPTION + if (flags) { + SWIG_Python_TypeError(SWIG_TypePrettyName(ty), obj); + SWIG_Python_ArgFail(argnum); + } +#endif + } + return result; +} + +#ifdef SWIGPYTHON_BUILTIN +SWIGRUNTIME int +SWIG_Python_NonDynamicSetAttr(PyObject *obj, PyObject *name, PyObject *value) { + PyTypeObject *tp = obj->ob_type; + PyObject *descr; + PyObject *encoded_name; + descrsetfunc f; + int res = -1; + +# ifdef Py_USING_UNICODE + if (PyString_Check(name)) { + name = PyUnicode_Decode(PyString_AsString(name), PyString_Size(name), NULL, NULL); + if (!name) + return -1; + } else if (!PyUnicode_Check(name)) +# else + if (!PyString_Check(name)) +# endif + { + PyErr_Format(PyExc_TypeError, "attribute name must be string, not '%.200s'", name->ob_type->tp_name); + return -1; + } else { + Py_INCREF(name); + } + + if (!tp->tp_dict) { + if (PyType_Ready(tp) < 0) + goto done; + } + + descr = _PyType_Lookup(tp, name); + f = NULL; + if (descr != NULL) + f = descr->ob_type->tp_descr_set; + if (!f) { + if (PyString_Check(name)) { + encoded_name = name; + Py_INCREF(name); + } else { + encoded_name = PyUnicode_AsUTF8String(name); + if (!encoded_name) + return -1; + } + PyErr_Format(PyExc_AttributeError, "'%.100s' object has no attribute '%.200s'", tp->tp_name, PyString_AsString(encoded_name)); + Py_DECREF(encoded_name); + } else { + res = f(descr, obj, value); + } + + done: + Py_DECREF(name); + return res; +} +#endif + + +#ifdef __cplusplus +} +#endif + + + +#define SWIG_exception_fail(code, msg) do { SWIG_Error(code, msg); SWIG_fail; } while(0) + +#define SWIG_contract_assert(expr, msg) if (!(expr)) { SWIG_Error(SWIG_RuntimeError, msg); SWIG_fail; } else + + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Method creation and docstring support functions */ + +SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name); +SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); +SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func); + +#ifdef __cplusplus +} +#endif + + +/* -------- TYPES TABLE (BEGIN) -------- */ + +#define SWIGTYPE_p_ControlMode swig_types[0] +#define SWIGTYPE_p_CoordinateSystem swig_types[1] +#define SWIGTYPE_p_EngineType swig_types[2] +#define SWIGTYPE_p_IndicatorState swig_types[3] +#define SWIGTYPE_p_Mode swig_types[4] +#define SWIGTYPE_p_SimulationMode swig_types[5] +#define SWIGTYPE_p_Type swig_types[6] +#define SWIGTYPE_p_UserInputEvent swig_types[7] +#define SWIGTYPE_p_WbCameraRecognitionObject swig_types[8] +#define SWIGTYPE_p_WbContactPoint swig_types[9] +#define SWIGTYPE_p_WbLidarPoint swig_types[10] +#define SWIGTYPE_p_WbMouseState swig_types[11] +#define SWIGTYPE_p_WbRadarTarget swig_types[12] +#define SWIGTYPE_p_WheelIndex swig_types[13] +#define SWIGTYPE_p_WiperMode swig_types[14] +#define SWIGTYPE_p_char swig_types[15] +#define SWIGTYPE_p_webots__Car swig_types[16] +#define SWIGTYPE_p_webots__Driver swig_types[17] +#define SWIGTYPE_p_webots__Robot swig_types[18] +#define SWIGTYPE_p_webots__Supervisor swig_types[19] +static swig_type_info *swig_types[21]; +static swig_module_info swig_module = {swig_types, 20, 0, 0, 0, 0}; +#define SWIG_TypeQuery(name) SWIG_TypeQueryModule(&swig_module, &swig_module, name) +#define SWIG_MangledTypeQuery(name) SWIG_MangledTypeQueryModule(&swig_module, &swig_module, name) + +/* -------- TYPES TABLE (END) -------- */ + +#ifdef SWIG_TypeQuery +# undef SWIG_TypeQuery +#endif +#define SWIG_TypeQuery SWIG_Python_TypeQuery + +/*----------------------------------------------- + @(target):= _vehicle.so + ------------------------------------------------*/ +#if PY_VERSION_HEX >= 0x03000000 +# define SWIG_init PyInit__vehicle + +#else +# define SWIG_init init_vehicle + +#endif +#define SWIG_name "_vehicle" + +#define SWIGVERSION 0x040002 +#define SWIG_VERSION SWIGVERSION + + +#define SWIG_as_voidptr(a) const_cast< void * >(static_cast< const void * >(a)) +#define SWIG_as_voidptrptr(a) ((void)SWIG_as_voidptr(*a),reinterpret_cast< void** >(a)) + + +#include + + +namespace swig { + class SwigPtr_PyObject { + protected: + PyObject *_obj; + + public: + SwigPtr_PyObject() :_obj(0) + { + } + + SwigPtr_PyObject(const SwigPtr_PyObject& item) : _obj(item._obj) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + SwigPtr_PyObject(PyObject *obj, bool initial_ref = true) :_obj(obj) + { + if (initial_ref) { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + } + + SwigPtr_PyObject & operator=(const SwigPtr_PyObject& item) + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XINCREF(item._obj); + Py_XDECREF(_obj); + _obj = item._obj; + SWIG_PYTHON_THREAD_END_BLOCK; + return *this; + } + + ~SwigPtr_PyObject() + { + SWIG_PYTHON_THREAD_BEGIN_BLOCK; + Py_XDECREF(_obj); + SWIG_PYTHON_THREAD_END_BLOCK; + } + + operator PyObject *() const + { + return _obj; + } + + PyObject *operator->() const + { + return _obj; + } + }; +} + + +namespace swig { + struct SwigVar_PyObject : SwigPtr_PyObject { + SwigVar_PyObject(PyObject* obj = 0) : SwigPtr_PyObject(obj, false) { } + + SwigVar_PyObject & operator = (PyObject* obj) + { + Py_XDECREF(_obj); + _obj = obj; + return *this; + } + }; +} + + +#include +#include + + +SWIGINTERNINLINE PyObject* + SWIG_From_int (int value) +{ + return PyInt_FromLong((long) value); +} + + +SWIGINTERNINLINE PyObject* + SWIG_From_bool (bool value) +{ + return PyBool_FromLong(value ? 1 : 0); +} + + +SWIGINTERN int +SWIG_AsVal_double (PyObject *obj, double *val) +{ + int res = SWIG_TypeError; + if (PyFloat_Check(obj)) { + if (val) *val = PyFloat_AsDouble(obj); + return SWIG_OK; +#if PY_VERSION_HEX < 0x03000000 + } else if (PyInt_Check(obj)) { + if (val) *val = (double) PyInt_AsLong(obj); + return SWIG_OK; +#endif + } else if (PyLong_Check(obj)) { + double v = PyLong_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + double d = PyFloat_AsDouble(obj); + if (!PyErr_Occurred()) { + if (val) *val = d; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_AddCast(SWIG_OK)); + } else { + PyErr_Clear(); + } + } + } +#endif + return res; +} + + + #define SWIG_From_double PyFloat_FromDouble + + +#include +#if !defined(SWIG_NO_LLONG_MAX) +# if !defined(LLONG_MAX) && defined(__GNUC__) && defined (__LONG_LONG_MAX__) +# define LLONG_MAX __LONG_LONG_MAX__ +# define LLONG_MIN (-LLONG_MAX - 1LL) +# define ULLONG_MAX (LLONG_MAX * 2ULL + 1ULL) +# endif +#endif + + +#include + + +#include + + +SWIGINTERNINLINE int +SWIG_CanCastAsInteger(double *d, double min, double max) { + double x = *d; + if ((min <= x && x <= max)) { + double fx = floor(x); + double cx = ceil(x); + double rd = ((x - fx) < 0.5) ? fx : cx; /* simple rint */ + if ((errno == EDOM) || (errno == ERANGE)) { + errno = 0; + } else { + double summ, reps, diff; + if (rd < x) { + diff = x - rd; + } else if (rd > x) { + diff = rd - x; + } else { + return 1; + } + summ = rd + x; + reps = diff/summ; + if (reps < 8*DBL_EPSILON) { + *d = rd; + return 1; + } + } + } + return 0; +} + + +SWIGINTERN int +SWIG_AsVal_long (PyObject *obj, long* val) +{ +#if PY_VERSION_HEX < 0x03000000 + if (PyInt_Check(obj)) { + if (val) *val = PyInt_AsLong(obj); + return SWIG_OK; + } else +#endif + if (PyLong_Check(obj)) { + long v = PyLong_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_OK; + } else { + PyErr_Clear(); + return SWIG_OverflowError; + } + } +#ifdef SWIG_PYTHON_CAST_MODE + { + int dispatch = 0; + long v = PyInt_AsLong(obj); + if (!PyErr_Occurred()) { + if (val) *val = v; + return SWIG_AddCast(SWIG_OK); + } else { + PyErr_Clear(); + } + if (!dispatch) { + double d; + int res = SWIG_AddCast(SWIG_AsVal_double (obj,&d)); + if (SWIG_IsOK(res) && SWIG_CanCastAsInteger(&d, LONG_MIN, LONG_MAX)) { + if (val) *val = (long)(d); + return res; + } + } + } +#endif + return SWIG_TypeError; +} + + +SWIGINTERN int +SWIG_AsVal_int (PyObject * obj, int *val) +{ + long v; + int res = SWIG_AsVal_long (obj, &v); + if (SWIG_IsOK(res)) { + if ((v < INT_MIN || v > INT_MAX)) { + return SWIG_OverflowError; + } else { + if (val) *val = static_cast< int >(v); + } + } + return res; +} + + +SWIGINTERN int +SWIG_AsVal_bool (PyObject *obj, bool *val) +{ + int r; + if (!PyBool_Check(obj)) + return SWIG_ERROR; + r = PyObject_IsTrue(obj); + if (r == -1) + return SWIG_ERROR; + if (val) *val = r ? true : false; + return SWIG_OK; +} + +#ifdef __cplusplus +extern "C" { +#endif +SWIGINTERN PyObject *_wrap_new_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Driver", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)new webots::Driver(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDriverInstance(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_getDriverInstance", 0, 0, 0)) SWIG_fail; + result = (webots::Driver *)webots::Driver::getDriverInstance(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Driver, 0 | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Driver(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Driver" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_isInitialisationPossible(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + bool result; + + if (!SWIG_Python_UnpackTuple(args, "Driver_isInitialisationPossible", 0, 0, 0)) SWIG_fail; + result = (bool)webots::Driver::isInitialisationPossible(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_step(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_step" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->step(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getSteeringAngle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setCruisingSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setCruisingSpeed" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setCruisingSpeed(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getTargetCruisingSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getTargetCruisingSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getTargetCruisingSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getCurrentSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getCurrentSpeed" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getCurrentSpeed(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setThrottle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setThrottle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setThrottle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getThrottle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getThrottle" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getThrottle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrakeIntensity", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrakeIntensity" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrakeIntensity(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getBrakeIntensity(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getBrakeIntensity" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getBrakeIntensity(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::IndicatorState arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setIndicator", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setIndicator" "', argument " "2"" of type '" "webots::Driver::IndicatorState""'"); + } + arg2 = static_cast< webots::Driver::IndicatorState >(val2); + (arg1)->setIndicator(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setHazardFlashers", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setHazardFlashers" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setHazardFlashers(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getIndicator(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::IndicatorState result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getIndicator" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::IndicatorState)(arg1)->getIndicator(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getHazardFlashers(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getHazardFlashers" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getHazardFlashers(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setDippedBeams", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setDippedBeams" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setDippedBeams(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setAntifogLights", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setAntifogLights" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->setAntifogLights(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getDippedBeams(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getDippedBeams" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getDippedBeams(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getAntifogLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getAntifogLights" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (bool)(arg1)->getAntifogLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getRpm(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getRpm" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (double)(arg1)->getRpm(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGear(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setGear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + int arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setGear", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setGear" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setGear" "', argument " "2"" of type '" "int""'"); + } + arg2 = static_cast< int >(val2); + (arg1)->setGear(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getGearNumber(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + int result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getGearNumber" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (int)(arg1)->getGearNumber(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getControlMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::ControlMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getControlMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::ControlMode)(arg1)->getControlMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWiperMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWiperMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWiperMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWiperMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWiperMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWiperMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setBrake(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setBrake", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setBrake" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setBrake" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setBrake(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_setWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + webots::Driver::WiperMode arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Driver_setWipersMode", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_setWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Driver_setWipersMode" "', argument " "2"" of type '" "webots::Driver::WiperMode""'"); + } + arg2 = static_cast< webots::Driver::WiperMode >(val2); + (arg1)->setWipersMode(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Driver_getWipersMode(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Driver *arg1 = (webots::Driver *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Driver::WiperMode result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Driver, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Driver_getWipersMode" "', argument " "1"" of type '" "webots::Driver *""'"); + } + arg1 = reinterpret_cast< webots::Driver * >(argp1); + result = (webots::Driver::WiperMode)(arg1)->getWipersMode(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Driver_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Driver, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Driver_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +SWIGINTERN PyObject *_wrap_new_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *result = 0 ; + + if (!SWIG_Python_UnpackTuple(args, "new_Car", 0, 0, 0)) SWIG_fail; + result = (webots::Car *)new webots::Car(); + resultobj = SWIG_NewPointerObj(SWIG_as_voidptr(result), SWIGTYPE_p_webots__Car, SWIG_POINTER_NEW | 0 ); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_delete_Car(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, SWIG_POINTER_DISOWN | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "delete_Car" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + delete arg1; + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::Type result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::Type)(arg1)->getType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getEngineType(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + webots::Car::EngineType result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getEngineType" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (webots::Car::EngineType)(arg1)->getEngineType(); + resultobj = SWIG_From_int(static_cast< int >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setIndicatorPeriod", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setIndicatorPeriod" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setIndicatorPeriod(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getIndicatorPeriod(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getIndicatorPeriod" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getIndicatorPeriod(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBackwardsLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBackwardsLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBackwardsLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getBrakeLights(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + bool result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getBrakeLights" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (bool)(arg1)->getBrakeLights(); + resultobj = SWIG_From_bool(static_cast< bool >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackFront(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackFront" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackFront(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getTrackRear(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getTrackRear" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getTrackRear(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelbase(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelbase" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getWheelbase(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getFrontWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getFrontWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getFrontWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRearWheelRadius(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRearWheelRadius" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRearWheelRadius(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelEncoder(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelEncoder", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelEncoder" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelEncoder" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelEncoder(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getWheelSpeed(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + webots::Car::WheelIndex arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + int val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + double result; + + if (!SWIG_Python_UnpackTuple(args, "Car_getWheelSpeed", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getWheelSpeed" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_int(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_getWheelSpeed" "', argument " "2"" of type '" "webots::Car::WheelIndex""'"); + } + arg2 = static_cast< webots::Car::WheelIndex >(val2); + result = (double)(arg1)->getWheelSpeed(arg2); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setLeftSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setLeftSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setLeftSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_setRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + double arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + double val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_setRightSteeringAngle", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_setRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_double(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_setRightSteeringAngle" "', argument " "2"" of type '" "double""'"); + } + arg2 = static_cast< double >(val2); + (arg1)->setRightSteeringAngle(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getRightSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getRightSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getRightSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_getLeftSteeringAngle(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + void *argp1 = 0 ; + int res1 = 0 ; + PyObject *swig_obj[1] ; + double result; + + if (!args) SWIG_fail; + swig_obj[0] = args; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_getLeftSteeringAngle" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + result = (double)(arg1)->getLeftSteeringAngle(); + resultobj = SWIG_From_double(static_cast< double >(result)); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableLimitedSlipDifferential(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableLimitedSlipDifferential", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableLimitedSlipDifferential" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableLimitedSlipDifferential(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *_wrap_Car_enableIndicatorAutoDisabling(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *resultobj = 0; + webots::Car *arg1 = (webots::Car *) 0 ; + bool arg2 ; + void *argp1 = 0 ; + int res1 = 0 ; + bool val2 ; + int ecode2 = 0 ; + PyObject *swig_obj[2] ; + + if (!SWIG_Python_UnpackTuple(args, "Car_enableIndicatorAutoDisabling", 2, 2, swig_obj)) SWIG_fail; + res1 = SWIG_ConvertPtr(swig_obj[0], &argp1,SWIGTYPE_p_webots__Car, 0 | 0 ); + if (!SWIG_IsOK(res1)) { + SWIG_exception_fail(SWIG_ArgError(res1), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "1"" of type '" "webots::Car *""'"); + } + arg1 = reinterpret_cast< webots::Car * >(argp1); + ecode2 = SWIG_AsVal_bool(swig_obj[1], &val2); + if (!SWIG_IsOK(ecode2)) { + SWIG_exception_fail(SWIG_ArgError(ecode2), "in method '" "Car_enableIndicatorAutoDisabling" "', argument " "2"" of type '" "bool""'"); + } + arg2 = static_cast< bool >(val2); + (arg1)->enableIndicatorAutoDisabling(arg2); + resultobj = SWIG_Py_Void(); + return resultobj; +fail: + return NULL; +} + + +SWIGINTERN PyObject *Car_swigregister(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + PyObject *obj; + if (!SWIG_Python_UnpackTuple(args, "swigregister", 1, 1, &obj)) return NULL; + SWIG_TypeNewClientData(SWIGTYPE_p_webots__Car, SWIG_NewClientData(obj)); + return SWIG_Py_Void(); +} + +SWIGINTERN PyObject *Car_swiginit(PyObject *SWIGUNUSEDPARM(self), PyObject *args) { + return SWIG_Python_InitShadowInstance(args); +} + +static PyMethodDef SwigMethods[] = { + { "SWIG_PyInstanceMethod_New", SWIG_PyInstanceMethod_New, METH_O, NULL}, + { "new_Driver", _wrap_new_Driver, METH_NOARGS, NULL}, + { "Driver_getDriverInstance", _wrap_Driver_getDriverInstance, METH_NOARGS, NULL}, + { "delete_Driver", _wrap_delete_Driver, METH_O, NULL}, + { "Driver_isInitialisationPossible", _wrap_Driver_isInitialisationPossible, METH_NOARGS, NULL}, + { "Driver_step", _wrap_Driver_step, METH_O, NULL}, + { "Driver_setSteeringAngle", _wrap_Driver_setSteeringAngle, METH_VARARGS, NULL}, + { "Driver_getSteeringAngle", _wrap_Driver_getSteeringAngle, METH_O, NULL}, + { "Driver_setCruisingSpeed", _wrap_Driver_setCruisingSpeed, METH_VARARGS, NULL}, + { "Driver_getTargetCruisingSpeed", _wrap_Driver_getTargetCruisingSpeed, METH_O, NULL}, + { "Driver_getCurrentSpeed", _wrap_Driver_getCurrentSpeed, METH_O, NULL}, + { "Driver_setThrottle", _wrap_Driver_setThrottle, METH_VARARGS, NULL}, + { "Driver_getThrottle", _wrap_Driver_getThrottle, METH_O, NULL}, + { "Driver_setBrakeIntensity", _wrap_Driver_setBrakeIntensity, METH_VARARGS, NULL}, + { "Driver_getBrakeIntensity", _wrap_Driver_getBrakeIntensity, METH_O, NULL}, + { "Driver_setIndicator", _wrap_Driver_setIndicator, METH_VARARGS, NULL}, + { "Driver_setHazardFlashers", _wrap_Driver_setHazardFlashers, METH_VARARGS, NULL}, + { "Driver_getIndicator", _wrap_Driver_getIndicator, METH_O, NULL}, + { "Driver_getHazardFlashers", _wrap_Driver_getHazardFlashers, METH_O, NULL}, + { "Driver_setDippedBeams", _wrap_Driver_setDippedBeams, METH_VARARGS, NULL}, + { "Driver_setAntifogLights", _wrap_Driver_setAntifogLights, METH_VARARGS, NULL}, + { "Driver_getDippedBeams", _wrap_Driver_getDippedBeams, METH_O, NULL}, + { "Driver_getAntifogLights", _wrap_Driver_getAntifogLights, METH_O, NULL}, + { "Driver_getRpm", _wrap_Driver_getRpm, METH_O, NULL}, + { "Driver_getGear", _wrap_Driver_getGear, METH_O, NULL}, + { "Driver_setGear", _wrap_Driver_setGear, METH_VARARGS, NULL}, + { "Driver_getGearNumber", _wrap_Driver_getGearNumber, METH_O, NULL}, + { "Driver_getControlMode", _wrap_Driver_getControlMode, METH_O, NULL}, + { "Driver_setWiperMode", _wrap_Driver_setWiperMode, METH_VARARGS, NULL}, + { "Driver_getWiperMode", _wrap_Driver_getWiperMode, METH_O, NULL}, + { "Driver_setBrake", _wrap_Driver_setBrake, METH_VARARGS, NULL}, + { "Driver_setWipersMode", _wrap_Driver_setWipersMode, METH_VARARGS, NULL}, + { "Driver_getWipersMode", _wrap_Driver_getWipersMode, METH_O, NULL}, + { "Driver_swigregister", Driver_swigregister, METH_O, NULL}, + { "Driver_swiginit", Driver_swiginit, METH_VARARGS, NULL}, + { "new_Car", _wrap_new_Car, METH_NOARGS, NULL}, + { "delete_Car", _wrap_delete_Car, METH_O, NULL}, + { "Car_getType", _wrap_Car_getType, METH_O, NULL}, + { "Car_getEngineType", _wrap_Car_getEngineType, METH_O, NULL}, + { "Car_setIndicatorPeriod", _wrap_Car_setIndicatorPeriod, METH_VARARGS, NULL}, + { "Car_getIndicatorPeriod", _wrap_Car_getIndicatorPeriod, METH_O, NULL}, + { "Car_getBackwardsLights", _wrap_Car_getBackwardsLights, METH_O, NULL}, + { "Car_getBrakeLights", _wrap_Car_getBrakeLights, METH_O, NULL}, + { "Car_getTrackFront", _wrap_Car_getTrackFront, METH_O, NULL}, + { "Car_getTrackRear", _wrap_Car_getTrackRear, METH_O, NULL}, + { "Car_getWheelbase", _wrap_Car_getWheelbase, METH_O, NULL}, + { "Car_getFrontWheelRadius", _wrap_Car_getFrontWheelRadius, METH_O, NULL}, + { "Car_getRearWheelRadius", _wrap_Car_getRearWheelRadius, METH_O, NULL}, + { "Car_getWheelEncoder", _wrap_Car_getWheelEncoder, METH_VARARGS, NULL}, + { "Car_getWheelSpeed", _wrap_Car_getWheelSpeed, METH_VARARGS, NULL}, + { "Car_setLeftSteeringAngle", _wrap_Car_setLeftSteeringAngle, METH_VARARGS, NULL}, + { "Car_setRightSteeringAngle", _wrap_Car_setRightSteeringAngle, METH_VARARGS, NULL}, + { "Car_getRightSteeringAngle", _wrap_Car_getRightSteeringAngle, METH_O, NULL}, + { "Car_getLeftSteeringAngle", _wrap_Car_getLeftSteeringAngle, METH_O, NULL}, + { "Car_enableLimitedSlipDifferential", _wrap_Car_enableLimitedSlipDifferential, METH_VARARGS, NULL}, + { "Car_enableIndicatorAutoDisabling", _wrap_Car_enableIndicatorAutoDisabling, METH_VARARGS, NULL}, + { "Car_swigregister", Car_swigregister, METH_O, NULL}, + { "Car_swiginit", Car_swiginit, METH_VARARGS, NULL}, + { NULL, NULL, 0, NULL } +}; + +static PyMethodDef SwigMethods_proxydocs[] = { + { NULL, NULL, 0, NULL } +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (BEGIN) -------- */ + +static void *_p_webots__DriverTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__CarTo_p_webots__Supervisor(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Supervisor *) (webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__DriverTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *) ((webots::Driver *) x)); +} +static void *_p_webots__SupervisorTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) ((webots::Supervisor *) x)); +} +static void *_p_webots__CarTo_p_webots__Robot(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Robot *) (webots::Supervisor *)(webots::Driver *) ((webots::Car *) x)); +} +static void *_p_webots__CarTo_p_webots__Driver(void *x, int *SWIGUNUSEDPARM(newmemory)) { + return (void *)((webots::Driver *) ((webots::Car *) x)); +} +static swig_type_info _swigt__p_ControlMode = {"_p_ControlMode", "ControlMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_CoordinateSystem = {"_p_CoordinateSystem", "CoordinateSystem *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_EngineType = {"_p_EngineType", "EngineType *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_IndicatorState = {"_p_IndicatorState", "IndicatorState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Mode = {"_p_Mode", "Mode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_SimulationMode = {"_p_SimulationMode", "SimulationMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_Type = {"_p_Type", "Type *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_UserInputEvent = {"_p_UserInputEvent", "UserInputEvent *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbCameraRecognitionObject = {"_p_WbCameraRecognitionObject", "WbCameraRecognitionObject *|webots::CameraRecognitionObject *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbContactPoint = {"_p_WbContactPoint", "WbContactPoint *|webots::ContactPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbLidarPoint = {"_p_WbLidarPoint", "WbLidarPoint *|webots::LidarPoint *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbMouseState = {"_p_WbMouseState", "WbMouseState *|webots::MouseState *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WbRadarTarget = {"_p_WbRadarTarget", "WbRadarTarget *|webots::RadarTarget *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WheelIndex = {"_p_WheelIndex", "WheelIndex *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_WiperMode = {"_p_WiperMode", "WiperMode *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_char = {"_p_char", "char *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Car = {"_p_webots__Car", "webots::Car *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Driver = {"_p_webots__Driver", "webots::Driver *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Robot = {"_p_webots__Robot", "webots::Robot *", 0, 0, (void*)0, 0}; +static swig_type_info _swigt__p_webots__Supervisor = {"_p_webots__Supervisor", "webots::Supervisor *", 0, 0, (void*)0, 0}; + +static swig_type_info *swig_type_initial[] = { + &_swigt__p_ControlMode, + &_swigt__p_CoordinateSystem, + &_swigt__p_EngineType, + &_swigt__p_IndicatorState, + &_swigt__p_Mode, + &_swigt__p_SimulationMode, + &_swigt__p_Type, + &_swigt__p_UserInputEvent, + &_swigt__p_WbCameraRecognitionObject, + &_swigt__p_WbContactPoint, + &_swigt__p_WbLidarPoint, + &_swigt__p_WbMouseState, + &_swigt__p_WbRadarTarget, + &_swigt__p_WheelIndex, + &_swigt__p_WiperMode, + &_swigt__p_char, + &_swigt__p_webots__Car, + &_swigt__p_webots__Driver, + &_swigt__p_webots__Robot, + &_swigt__p_webots__Supervisor, +}; + +static swig_cast_info _swigc__p_ControlMode[] = { {&_swigt__p_ControlMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_CoordinateSystem[] = { {&_swigt__p_CoordinateSystem, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_EngineType[] = { {&_swigt__p_EngineType, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_IndicatorState[] = { {&_swigt__p_IndicatorState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Mode[] = { {&_swigt__p_Mode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_SimulationMode[] = { {&_swigt__p_SimulationMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_Type[] = { {&_swigt__p_Type, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_UserInputEvent[] = { {&_swigt__p_UserInputEvent, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbCameraRecognitionObject[] = { {&_swigt__p_WbCameraRecognitionObject, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbContactPoint[] = { {&_swigt__p_WbContactPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbLidarPoint[] = { {&_swigt__p_WbLidarPoint, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbMouseState[] = { {&_swigt__p_WbMouseState, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WbRadarTarget[] = { {&_swigt__p_WbRadarTarget, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WheelIndex[] = { {&_swigt__p_WheelIndex, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_WiperMode[] = { {&_swigt__p_WiperMode, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_char[] = { {&_swigt__p_char, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Car[] = { {&_swigt__p_webots__Car, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Driver[] = { {&_swigt__p_webots__Driver, 0, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Driver, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Robot[] = { {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Supervisor, _p_webots__SupervisorTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Robot, 0, 0}, {&_swigt__p_webots__Robot, 0, 0, 0},{0, 0, 0, 0}}; +static swig_cast_info _swigc__p_webots__Supervisor[] = { {&_swigt__p_webots__Supervisor, 0, 0, 0}, {&_swigt__p_webots__Driver, _p_webots__DriverTo_p_webots__Supervisor, 0, 0}, {&_swigt__p_webots__Car, _p_webots__CarTo_p_webots__Supervisor, 0, 0},{0, 0, 0, 0}}; + +static swig_cast_info *swig_cast_initial[] = { + _swigc__p_ControlMode, + _swigc__p_CoordinateSystem, + _swigc__p_EngineType, + _swigc__p_IndicatorState, + _swigc__p_Mode, + _swigc__p_SimulationMode, + _swigc__p_Type, + _swigc__p_UserInputEvent, + _swigc__p_WbCameraRecognitionObject, + _swigc__p_WbContactPoint, + _swigc__p_WbLidarPoint, + _swigc__p_WbMouseState, + _swigc__p_WbRadarTarget, + _swigc__p_WheelIndex, + _swigc__p_WiperMode, + _swigc__p_char, + _swigc__p_webots__Car, + _swigc__p_webots__Driver, + _swigc__p_webots__Robot, + _swigc__p_webots__Supervisor, +}; + + +/* -------- TYPE CONVERSION AND EQUIVALENCE RULES (END) -------- */ + +static swig_const_info swig_const_table[] = { +{0, 0, 0, 0.0, 0, 0}}; + +#ifdef __cplusplus +} +#endif +/* ----------------------------------------------------------------------------- + * Type initialization: + * This problem is tough by the requirement that no dynamic + * memory is used. Also, since swig_type_info structures store pointers to + * swig_cast_info structures and swig_cast_info structures store pointers back + * to swig_type_info structures, we need some lookup code at initialization. + * The idea is that swig generates all the structures that are needed. + * The runtime then collects these partially filled structures. + * The SWIG_InitializeModule function takes these initial arrays out of + * swig_module, and does all the lookup, filling in the swig_module.types + * array with the correct data and linking the correct swig_cast_info + * structures together. + * + * The generated swig_type_info structures are assigned statically to an initial + * array. We just loop through that array, and handle each type individually. + * First we lookup if this type has been already loaded, and if so, use the + * loaded structure instead of the generated one. Then we have to fill in the + * cast linked list. The cast data is initially stored in something like a + * two-dimensional array. Each row corresponds to a type (there are the same + * number of rows as there are in the swig_type_initial array). Each entry in + * a column is one of the swig_cast_info structures for that type. + * The cast_initial array is actually an array of arrays, because each row has + * a variable number of columns. So to actually build the cast linked list, + * we find the array of casts associated with the type, and loop through it + * adding the casts to the list. The one last trick we need to do is making + * sure the type pointer in the swig_cast_info struct is correct. + * + * First off, we lookup the cast->type name to see if it is already loaded. + * There are three cases to handle: + * 1) If the cast->type has already been loaded AND the type we are adding + * casting info to has not been loaded (it is in this module), THEN we + * replace the cast->type pointer with the type pointer that has already + * been loaded. + * 2) If BOTH types (the one we are adding casting info to, and the + * cast->type) are loaded, THEN the cast info has already been loaded by + * the previous module so we just ignore it. + * 3) Finally, if cast->type has not already been loaded, then we add that + * swig_cast_info to the linked list (because the cast->type) pointer will + * be correct. + * ----------------------------------------------------------------------------- */ + +#ifdef __cplusplus +extern "C" { +#if 0 +} /* c-mode */ +#endif +#endif + +#if 0 +#define SWIGRUNTIME_DEBUG +#endif + + +SWIGRUNTIME void +SWIG_InitializeModule(void *clientdata) { + size_t i; + swig_module_info *module_head, *iter; + int init; + + /* check to see if the circular list has been setup, if not, set it up */ + if (swig_module.next==0) { + /* Initialize the swig_module */ + swig_module.type_initial = swig_type_initial; + swig_module.cast_initial = swig_cast_initial; + swig_module.next = &swig_module; + init = 1; + } else { + init = 0; + } + + /* Try and load any already created modules */ + module_head = SWIG_GetModule(clientdata); + if (!module_head) { + /* This is the first module loaded for this interpreter */ + /* so set the swig module into the interpreter */ + SWIG_SetModule(clientdata, &swig_module); + } else { + /* the interpreter has loaded a SWIG module, but has it loaded this one? */ + iter=module_head; + do { + if (iter==&swig_module) { + /* Our module is already in the list, so there's nothing more to do. */ + return; + } + iter=iter->next; + } while (iter!= module_head); + + /* otherwise we must add our module into the list */ + swig_module.next = module_head->next; + module_head->next = &swig_module; + } + + /* When multiple interpreters are used, a module could have already been initialized in + a different interpreter, but not yet have a pointer in this interpreter. + In this case, we do not want to continue adding types... everything should be + set up already */ + if (init == 0) return; + + /* Now work on filling in swig_module.types */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: size %lu\n", (unsigned long)swig_module.size); +#endif + for (i = 0; i < swig_module.size; ++i) { + swig_type_info *type = 0; + swig_type_info *ret; + swig_cast_info *cast; + +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); +#endif + + /* if there is another module already loaded */ + if (swig_module.next != &swig_module) { + type = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, swig_module.type_initial[i]->name); + } + if (type) { + /* Overwrite clientdata field */ +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found type %s\n", type->name); +#endif + if (swig_module.type_initial[i]->clientdata) { + type->clientdata = swig_module.type_initial[i]->clientdata; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: found and overwrite type %s \n", type->name); +#endif + } + } else { + type = swig_module.type_initial[i]; + } + + /* Insert casting types */ + cast = swig_module.cast_initial[i]; + while (cast->type) { + /* Don't need to add information already in the list */ + ret = 0; +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: look cast %s\n", cast->type->name); +#endif + if (swig_module.next != &swig_module) { + ret = SWIG_MangledTypeQueryModule(swig_module.next, &swig_module, cast->type->name); +#ifdef SWIGRUNTIME_DEBUG + if (ret) printf("SWIG_InitializeModule: found cast %s\n", ret->name); +#endif + } + if (ret) { + if (type == swig_module.type_initial[i]) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: skip old type %s\n", ret->name); +#endif + cast->type = ret; + ret = 0; + } else { + /* Check for casting already in the list */ + swig_cast_info *ocast = SWIG_TypeCheck(ret->name, type); +#ifdef SWIGRUNTIME_DEBUG + if (ocast) printf("SWIG_InitializeModule: skip old cast %s\n", ret->name); +#endif + if (!ocast) ret = 0; + } + } + + if (!ret) { +#ifdef SWIGRUNTIME_DEBUG + printf("SWIG_InitializeModule: adding cast %s\n", cast->type->name); +#endif + if (type->cast) { + type->cast->prev = cast; + cast->next = type->cast; + } + type->cast = cast; + } + cast++; + } + /* Set entry in modules->types array equal to the type */ + swig_module.types[i] = type; + } + swig_module.types[i] = 0; + +#ifdef SWIGRUNTIME_DEBUG + printf("**** SWIG_InitializeModule: Cast List ******\n"); + for (i = 0; i < swig_module.size; ++i) { + int j = 0; + swig_cast_info *cast = swig_module.cast_initial[i]; + printf("SWIG_InitializeModule: type %lu %s\n", (unsigned long)i, swig_module.type_initial[i]->name); + while (cast->type) { + printf("SWIG_InitializeModule: cast type %s\n", cast->type->name); + cast++; + ++j; + } + printf("---- Total casts: %d\n",j); + } + printf("**** SWIG_InitializeModule: Cast List ******\n"); +#endif +} + +/* This function will propagate the clientdata field of type to +* any new swig_type_info structures that have been added into the list +* of equivalent types. It is like calling +* SWIG_TypeClientData(type, clientdata) a second time. +*/ +SWIGRUNTIME void +SWIG_PropagateClientData(void) { + size_t i; + swig_cast_info *equiv; + static int init_run = 0; + + if (init_run) return; + init_run = 1; + + for (i = 0; i < swig_module.size; i++) { + if (swig_module.types[i]->clientdata) { + equiv = swig_module.types[i]->cast; + while (equiv) { + if (!equiv->converter) { + if (equiv->type && !equiv->type->clientdata) + SWIG_TypeClientData(equiv->type, swig_module.types[i]->clientdata); + } + equiv = equiv->next; + } + } + } +} + +#ifdef __cplusplus +#if 0 +{ + /* c-mode */ +#endif +} +#endif + + + +#ifdef __cplusplus +extern "C" { +#endif + + /* Python-specific SWIG API */ +#define SWIG_newvarlink() SWIG_Python_newvarlink() +#define SWIG_addvarlink(p, name, get_attr, set_attr) SWIG_Python_addvarlink(p, name, get_attr, set_attr) +#define SWIG_InstallConstants(d, constants) SWIG_Python_InstallConstants(d, constants) + + /* ----------------------------------------------------------------------------- + * global variable support code. + * ----------------------------------------------------------------------------- */ + + typedef struct swig_globalvar { + char *name; /* Name of global variable */ + PyObject *(*get_attr)(void); /* Return the current value */ + int (*set_attr)(PyObject *); /* Set the value */ + struct swig_globalvar *next; + } swig_globalvar; + + typedef struct swig_varlinkobject { + PyObject_HEAD + swig_globalvar *vars; + } swig_varlinkobject; + + SWIGINTERN PyObject * + swig_varlink_repr(swig_varlinkobject *SWIGUNUSEDPARM(v)) { +#if PY_VERSION_HEX >= 0x03000000 + return PyUnicode_InternFromString(""); +#else + return PyString_FromString(""); +#endif + } + + SWIGINTERN PyObject * + swig_varlink_str(swig_varlinkobject *v) { +#if PY_VERSION_HEX >= 0x03000000 + PyObject *str = PyUnicode_InternFromString("("); + PyObject *tail; + PyObject *joined; + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + tail = PyUnicode_FromString(var->name); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + if (var->next) { + tail = PyUnicode_InternFromString(", "); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; + } + } + tail = PyUnicode_InternFromString(")"); + joined = PyUnicode_Concat(str, tail); + Py_DecRef(str); + Py_DecRef(tail); + str = joined; +#else + PyObject *str = PyString_FromString("("); + swig_globalvar *var; + for (var = v->vars; var; var=var->next) { + PyString_ConcatAndDel(&str,PyString_FromString(var->name)); + if (var->next) PyString_ConcatAndDel(&str,PyString_FromString(", ")); + } + PyString_ConcatAndDel(&str,PyString_FromString(")")); +#endif + return str; + } + + SWIGINTERN void + swig_varlink_dealloc(swig_varlinkobject *v) { + swig_globalvar *var = v->vars; + while (var) { + swig_globalvar *n = var->next; + free(var->name); + free(var); + var = n; + } + } + + SWIGINTERN PyObject * + swig_varlink_getattr(swig_varlinkobject *v, char *n) { + PyObject *res = NULL; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->get_attr)(); + break; + } + var = var->next; + } + if (res == NULL && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN int + swig_varlink_setattr(swig_varlinkobject *v, char *n, PyObject *p) { + int res = 1; + swig_globalvar *var = v->vars; + while (var) { + if (strcmp(var->name,n) == 0) { + res = (*var->set_attr)(p); + break; + } + var = var->next; + } + if (res == 1 && !PyErr_Occurred()) { + PyErr_Format(PyExc_AttributeError, "Unknown C global variable '%s'", n); + } + return res; + } + + SWIGINTERN PyTypeObject* + swig_varlink_type(void) { + static char varlink__doc__[] = "Swig var link object"; + static PyTypeObject varlink_type; + static int type_init = 0; + if (!type_init) { + const PyTypeObject tmp = { +#if PY_VERSION_HEX >= 0x03000000 + PyVarObject_HEAD_INIT(NULL, 0) +#else + PyObject_HEAD_INIT(NULL) + 0, /* ob_size */ +#endif + "swigvarlink", /* tp_name */ + sizeof(swig_varlinkobject), /* tp_basicsize */ + 0, /* tp_itemsize */ + (destructor) swig_varlink_dealloc, /* tp_dealloc */ + 0, /* tp_print */ + (getattrfunc) swig_varlink_getattr, /* tp_getattr */ + (setattrfunc) swig_varlink_setattr, /* tp_setattr */ + 0, /* tp_compare */ + (reprfunc) swig_varlink_repr, /* tp_repr */ + 0, /* tp_as_number */ + 0, /* tp_as_sequence */ + 0, /* tp_as_mapping */ + 0, /* tp_hash */ + 0, /* tp_call */ + (reprfunc) swig_varlink_str, /* tp_str */ + 0, /* tp_getattro */ + 0, /* tp_setattro */ + 0, /* tp_as_buffer */ + 0, /* tp_flags */ + varlink__doc__, /* tp_doc */ + 0, /* tp_traverse */ + 0, /* tp_clear */ + 0, /* tp_richcompare */ + 0, /* tp_weaklistoffset */ + 0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0, /* tp_iter -> tp_weaklist */ + 0, /* tp_del */ + 0, /* tp_version_tag */ +#if PY_VERSION_HEX >= 0x03040000 + 0, /* tp_finalize */ +#endif +#if PY_VERSION_HEX >= 0x03080000 + 0, /* tp_vectorcall */ +#endif +#if (PY_VERSION_HEX >= 0x03080000) && (PY_VERSION_HEX < 0x03090000) + 0, /* tp_print */ +#endif +#ifdef COUNT_ALLOCS + 0, /* tp_allocs */ + 0, /* tp_frees */ + 0, /* tp_maxalloc */ + 0, /* tp_prev */ + 0 /* tp_next */ +#endif + }; + varlink_type = tmp; + type_init = 1; + if (PyType_Ready(&varlink_type) < 0) + return NULL; + } + return &varlink_type; + } + + /* Create a variable linking object for use later */ + SWIGINTERN PyObject * + SWIG_Python_newvarlink(void) { + swig_varlinkobject *result = PyObject_NEW(swig_varlinkobject, swig_varlink_type()); + if (result) { + result->vars = 0; + } + return ((PyObject*) result); + } + + SWIGINTERN void + SWIG_Python_addvarlink(PyObject *p, const char *name, PyObject *(*get_attr)(void), int (*set_attr)(PyObject *p)) { + swig_varlinkobject *v = (swig_varlinkobject *) p; + swig_globalvar *gv = (swig_globalvar *) malloc(sizeof(swig_globalvar)); + if (gv) { + size_t size = strlen(name)+1; + gv->name = (char *)malloc(size); + if (gv->name) { + memcpy(gv->name, name, size); + gv->get_attr = get_attr; + gv->set_attr = set_attr; + gv->next = v->vars; + } + } + v->vars = gv; + } + + SWIGINTERN PyObject * + SWIG_globals(void) { + static PyObject *globals = 0; + if (!globals) { + globals = SWIG_newvarlink(); + } + return globals; + } + + /* ----------------------------------------------------------------------------- + * constants/methods manipulation + * ----------------------------------------------------------------------------- */ + + /* Install Constants */ + SWIGINTERN void + SWIG_Python_InstallConstants(PyObject *d, swig_const_info constants[]) { + PyObject *obj = 0; + size_t i; + for (i = 0; constants[i].type; ++i) { + switch(constants[i].type) { + case SWIG_PY_POINTER: + obj = SWIG_InternalNewPointerObj(constants[i].pvalue, *(constants[i]).ptype,0); + break; + case SWIG_PY_BINARY: + obj = SWIG_NewPackedObj(constants[i].pvalue, constants[i].lvalue, *(constants[i].ptype)); + break; + default: + obj = 0; + break; + } + if (obj) { + PyDict_SetItemString(d, constants[i].name, obj); + Py_DECREF(obj); + } + } + } + + /* -----------------------------------------------------------------------------*/ + /* Fix SwigMethods to carry the callback ptrs when needed */ + /* -----------------------------------------------------------------------------*/ + + SWIGINTERN void + SWIG_Python_FixMethods(PyMethodDef *methods, + swig_const_info *const_table, + swig_type_info **types, + swig_type_info **types_initial) { + size_t i; + for (i = 0; methods[i].ml_name; ++i) { + const char *c = methods[i].ml_doc; + if (!c) continue; + c = strstr(c, "swig_ptr: "); + if (c) { + int j; + swig_const_info *ci = 0; + const char *name = c + 10; + for (j = 0; const_table[j].type; ++j) { + if (strncmp(const_table[j].name, name, + strlen(const_table[j].name)) == 0) { + ci = &(const_table[j]); + break; + } + } + if (ci) { + void *ptr = (ci->type == SWIG_PY_POINTER) ? ci->pvalue : 0; + if (ptr) { + size_t shift = (ci->ptype) - types; + swig_type_info *ty = types_initial[shift]; + size_t ldoc = (c - methods[i].ml_doc); + size_t lptr = strlen(ty->name)+2*sizeof(void*)+2; + char *ndoc = (char*)malloc(ldoc + lptr + 10); + if (ndoc) { + char *buff = ndoc; + memcpy(buff, methods[i].ml_doc, ldoc); + buff += ldoc; + memcpy(buff, "swig_ptr: ", 10); + buff += 10; + SWIG_PackVoidPtr(buff, ptr, ty->name, lptr); + methods[i].ml_doc = ndoc; + } + } + } + } + } + } + + /* ----------------------------------------------------------------------------- + * Method creation and docstring support functions + * ----------------------------------------------------------------------------- */ + + /* ----------------------------------------------------------------------------- + * Function to find the method definition with the correct docstring for the + * proxy module as opposed to the low-level API + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyMethodDef *SWIG_PythonGetProxyDoc(const char *name) { + /* Find the function in the modified method table */ + size_t offset = 0; + int found = 0; + while (SwigMethods_proxydocs[offset].ml_meth != NULL) { + if (strcmp(SwigMethods_proxydocs[offset].ml_name, name) == 0) { + found = 1; + break; + } + offset++; + } + /* Use the copy with the modified docstring if available */ + return found ? &SwigMethods_proxydocs[offset] : NULL; + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyInstanceMethod_New() used in Python 3 + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyInstanceMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } +#if PY_VERSION_HEX >= 0x03000000 + return PyInstanceMethod_New(func); +#else + return PyMethod_New(func, NULL, NULL); +#endif + } + + /* ----------------------------------------------------------------------------- + * Wrapper of PyStaticMethod_New() + * It is exported to the generated module, used for -fastproxy + * ----------------------------------------------------------------------------- */ + + SWIGINTERN PyObject *SWIG_PyStaticMethod_New(PyObject *SWIGUNUSEDPARM(self), PyObject *func) { + if (PyCFunction_Check(func)) { + PyCFunctionObject *funcobj = (PyCFunctionObject *)func; + PyMethodDef *ml = SWIG_PythonGetProxyDoc(funcobj->m_ml->ml_name); + if (ml) + func = PyCFunction_NewEx(ml, funcobj->m_self, funcobj->m_module); + } + return PyStaticMethod_New(func); + } + +#ifdef __cplusplus +} +#endif + +/* -----------------------------------------------------------------------------* + * Partial Init method + * -----------------------------------------------------------------------------*/ + +#ifdef __cplusplus +extern "C" +#endif + +SWIGEXPORT +#if PY_VERSION_HEX >= 0x03000000 +PyObject* +#else +void +#endif +SWIG_init(void) { + PyObject *m, *d, *md, *globals; + +#if PY_VERSION_HEX >= 0x03000000 + static struct PyModuleDef SWIG_module = { + PyModuleDef_HEAD_INIT, + SWIG_name, + NULL, + -1, + SwigMethods, + NULL, + NULL, + NULL, + NULL + }; +#endif + +#if defined(SWIGPYTHON_BUILTIN) + static SwigPyClientData SwigPyObject_clientdata = { + 0, 0, 0, 0, 0, 0, 0 + }; + static PyGetSetDef this_getset_def = { + (char *)"this", &SwigPyBuiltin_ThisClosure, NULL, NULL, NULL + }; + static SwigPyGetSet thisown_getset_closure = { + SwigPyObject_own, + SwigPyObject_own + }; + static PyGetSetDef thisown_getset_def = { + (char *)"thisown", SwigPyBuiltin_GetterClosure, SwigPyBuiltin_SetterClosure, NULL, &thisown_getset_closure + }; + PyTypeObject *builtin_pytype; + int builtin_base_count; + swig_type_info *builtin_basetype; + PyObject *tuple; + PyGetSetDescrObject *static_getset; + PyTypeObject *metatype; + PyTypeObject *swigpyobject; + SwigPyClientData *cd; + PyObject *public_interface, *public_symbol; + PyObject *this_descr; + PyObject *thisown_descr; + PyObject *self = 0; + int i; + + (void)builtin_pytype; + (void)builtin_base_count; + (void)builtin_basetype; + (void)tuple; + (void)static_getset; + (void)self; + + /* Metaclass is used to implement static member variables */ + metatype = SwigPyObjectType(); + assert(metatype); +#endif + + (void)globals; + + /* Create singletons now to avoid potential deadlocks with multi-threaded usage after module initialization */ + SWIG_This(); + SWIG_Python_TypeCache(); + SwigPyPacked_type(); +#ifndef SWIGPYTHON_BUILTIN + SwigPyObject_type(); +#endif + + /* Fix SwigMethods to carry the callback ptrs when needed */ + SWIG_Python_FixMethods(SwigMethods, swig_const_table, swig_types, swig_type_initial); + +#if PY_VERSION_HEX >= 0x03000000 + m = PyModule_Create(&SWIG_module); +#else + m = Py_InitModule(SWIG_name, SwigMethods); +#endif + + md = d = PyModule_GetDict(m); + (void)md; + + SWIG_InitializeModule(0); + +#ifdef SWIGPYTHON_BUILTIN + swigpyobject = SwigPyObject_TypeOnce(); + + SwigPyObject_stype = SWIG_MangledTypeQuery("_p_SwigPyObject"); + assert(SwigPyObject_stype); + cd = (SwigPyClientData*) SwigPyObject_stype->clientdata; + if (!cd) { + SwigPyObject_stype->clientdata = &SwigPyObject_clientdata; + SwigPyObject_clientdata.pytype = swigpyobject; + } else if (swigpyobject->tp_basicsize != cd->pytype->tp_basicsize) { + PyErr_SetString(PyExc_RuntimeError, "Import error: attempted to load two incompatible swig-generated modules."); +# if PY_VERSION_HEX >= 0x03000000 + return NULL; +# else + return; +# endif + } + + /* All objects have a 'this' attribute */ + this_descr = PyDescr_NewGetSet(SwigPyObject_type(), &this_getset_def); + (void)this_descr; + + /* All objects have a 'thisown' attribute */ + thisown_descr = PyDescr_NewGetSet(SwigPyObject_type(), &thisown_getset_def); + (void)thisown_descr; + + public_interface = PyList_New(0); + public_symbol = 0; + (void)public_symbol; + + PyDict_SetItemString(md, "__all__", public_interface); + Py_DECREF(public_interface); + for (i = 0; SwigMethods[i].ml_name != NULL; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, SwigMethods[i].ml_name); + for (i = 0; swig_const_table[i].name != 0; ++i) + SwigPyBuiltin_AddPublicSymbol(public_interface, swig_const_table[i].name); +#endif + + SWIG_InstallConstants(d,swig_const_table); + + SWIG_Python_SetConstant(d, "Driver_INDICATOR_OFF",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_OFF))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_RIGHT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_RIGHT))); + SWIG_Python_SetConstant(d, "Driver_INDICATOR_LEFT",SWIG_From_int(static_cast< int >(webots::Driver::INDICATOR_LEFT))); + SWIG_Python_SetConstant(d, "Driver_SPEED",SWIG_From_int(static_cast< int >(webots::Driver::SPEED))); + SWIG_Python_SetConstant(d, "Driver_TORQUE",SWIG_From_int(static_cast< int >(webots::Driver::TORQUE))); + SWIG_Python_SetConstant(d, "Driver_DOWN",SWIG_From_int(static_cast< int >(webots::Driver::DOWN))); + SWIG_Python_SetConstant(d, "Driver_SLOW",SWIG_From_int(static_cast< int >(webots::Driver::SLOW))); + SWIG_Python_SetConstant(d, "Driver_NORMAL",SWIG_From_int(static_cast< int >(webots::Driver::NORMAL))); + SWIG_Python_SetConstant(d, "Driver_FAST",SWIG_From_int(static_cast< int >(webots::Driver::FAST))); + SWIG_Python_SetConstant(d, "Car_TRACTION",SWIG_From_int(static_cast< int >(webots::Car::TRACTION))); + SWIG_Python_SetConstant(d, "Car_PROPULSION",SWIG_From_int(static_cast< int >(webots::Car::PROPULSION))); + SWIG_Python_SetConstant(d, "Car_FOUR_BY_FOUR",SWIG_From_int(static_cast< int >(webots::Car::FOUR_BY_FOUR))); + SWIG_Python_SetConstant(d, "Car_COMBUSTION_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::COMBUSTION_ENGINE))); + SWIG_Python_SetConstant(d, "Car_ELECTRIC_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::ELECTRIC_ENGINE))); + SWIG_Python_SetConstant(d, "Car_PARALLEL_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::PARALLEL_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_POWER_SPLIT_HYBRID_ENGINE",SWIG_From_int(static_cast< int >(webots::Car::POWER_SPLIT_HYBRID_ENGINE))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_FRONT_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_FRONT_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_RIGHT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_RIGHT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_REAR_LEFT",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_REAR_LEFT))); + SWIG_Python_SetConstant(d, "Car_WHEEL_NB",SWIG_From_int(static_cast< int >(webots::Car::WHEEL_NB))); +#if PY_VERSION_HEX >= 0x03000000 + return m; +#else + return; +#endif +} + diff --git a/webots_ros2_driver/webots/resources/Makefile.include b/webots_ros2_driver/webots/resources/Makefile.include new file mode 100644 index 000000000..1d10d985b --- /dev/null +++ b/webots_ros2_driver/webots/resources/Makefile.include @@ -0,0 +1,670 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +### Generic Makefile.include for Webots controllers, physics plugins, robot +### window libraries, remote control libraries and other libraries +### to be used with GNU make +### +### Platforms: Windows, macOS, Linux +### Languages: C, C++ +### +### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer +### Edmund Ronald, Sergei Poskriakov +### +###----------------------------------------------------------------------------- +### +### This file is meant to be included from the Makefile files located in the +### Webots projects subdirectories. It is possible to set a number of variables +### to customize the build process, i.e., add source files, compilation flags, +### include paths, libraries, etc. These variables should be set in your local +### Makefile just before including this Makefile.include. This Makefile.include +### should never be modified. +### +### Here is a description of the variables you may set in your local Makefile: +### +### ---- C Sources ---- +### if your program uses several C source files: +### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c +### +### ---- C++ Sources ---- +### if your program uses several C++ source files: +### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++ +### +### ---- Compilation options ---- +### if special compilation flags are necessary: +### CFLAGS = -Wno-unused-result +### +### ---- Linked libraries ---- +### if your program needs additional libraries: +### INCLUDE = -I"/my_library_path/include" +### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library +### +### ---- Linking options ---- +### if special linking flags are needed: +### LFLAGS = -s +### +### ---- Webots included libraries ---- +### if you want to use the Webots C API in your C++ controller program: +### USE_C_API = true +### +### ---- Debug mode ---- +### if you want to display the gcc command line for compilation and link, as +### well as the rm command details used for cleaning: +### VERBOSE = 1 +### +###----------------------------------------------------------------------------- + + +ifdef WEBOTS_HOME + # Check if WEBOTS_HOME contains a trailing path separator and display a warning in this case. + PROTECTED_WEBOTS_HOME := $(WEBOTS_HOME:/=) + PROTECTED_WEBOTS_HOME := $(PROTECTED_WEBOTS_HOME:\=) + ifneq ($(WEBOTS_HOME),$(PROTECTED_WEBOTS_HOME)) + WEBOTS_HOME := $(PROTECTED_WEBOTS_HOME) + $(warning Please remove the trailing path separator from WEBOTS_HOME.) + endif +else + # WEBOTS_HOME is a sine qua non condition to run this Makefile. + $(error The WEBOTS_HOME environment variable is not defined.) +endif + +# WEBOTS_HOME_PATH is commonly defined in the caller Makefile +# but as it is not necessary on non-windows OS, it seems safer to reconstruct it there if required +null := +space := $(null) $(null) +WEBOTS_HOME_PATH ?= $(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) + +###----------------------------------------------------------------------------- +### OS determination and OS specific commands +###----------------------------------------------------------------------------- + +include $(WEBOTS_HOME_PATH)/resources/Makefile.os.include + +###----------------------------------------------------------------------------- +### Functions +###----------------------------------------------------------------------------- + +ifeq ($(OSTYPE),darwin) + + # Retrieve the relative path of a path according to some base path. + # cf. http://stackoverflow.com/a/3344050/2210777 + # @param 1 Base path (e.g. "/Users/fabien/develop/webots") + # @param 2 Path (e.g. "/Users/fabien/develop/webots/projects/robots/e-puck/controllers/e-puck") + # @return The relative path (e.g. "../../../../../") + computeRelativePath = $(shell \ + basedir=$(1); \ + currentdir=$(2); \ + if [[ $$currentdir =~ ^$$basedir ]] ; \ + then \ + up=; \ + while ! expr $$basedir : $$currentdir >/dev/null; do \ + up=../$$up; \ + currentdir=`dirname $$currentdir`; \ + done; \ + relative=$$up`expr $$basedir : $$currentdir'/*\(.*\)'`; \ + echo $$relative; \ + else \ + echo $$currentdir; \ + fi; \ + ) + +ifndef NO_FAT_BINARY +FAT_BINARY = 1 +endif + +endif + +###----------------------------------------------------------------------------- +### Paths +###----------------------------------------------------------------------------- + +# compiler +ifeq ($(CC),cc) + CC = gcc +endif + +# compute the current directory, the CURDIR variable may be already set +ifndef CURDIR + CURDIR = $(shell pwd) +endif + +# compute the name of the controller from its directory (i.e. braiten) +BAD_NAME = $(basename $(notdir $(CURDIR))) + +# this is a hack to work around a problem with spaces in dir names +NAME = $(word $(words $(BAD_NAME)),$(BAD_NAME)) + +# compute BUILD_TYPE (either ode, controllers, physics, robot_windows, remote_controls or libraries) +SPLIT_PATH = $(subst /, ,$(dir $(CURDIR))) +BUILD_TYPE = $(word $(words $(SPLIT_PATH)),$(SPLIT_PATH)) + +ifdef VERBOSE + SILENT = +else + SILENT = @ +endif + +# backward compatibility +ifdef CC_SOURCES + $(warning Please use CXX_SOURCES instead of CC_SOURCES to define the C++ sources) +endif +ifdef CPP_SOURCES + $(warning Please use CXX_SOURCES instead of CPP_SOURCES to define the C++ sources) +endif +CXX_SOURCES += $(CC_SOURCES) $(CPP_SOURCES) + +# if the source files were not explicitly defined, we try to find +# the *.c, *.cc, *.c++, or *.cpp source files. +ifeq ($(C_SOURCES),) + ifeq ($(strip $(CXX_SOURCES)),) + C_SOURCES = $(shell ls $(NAME).c 2> /dev/null) + CXX_SOURCES = $(shell ls $(NAME).cpp 2> /dev/null) + ifeq ($(CXX_SOURCES),) + CXX_SOURCES = $(shell ls $(NAME).cc 2> /dev/null) + ifeq ($(CXX_SOURCES),) + CXX_SOURCES = $(shell ls $(NAME).c++ 2> /dev/null) + endif + endif + endif +endif + +ifneq ($(strip $(CXX_SOURCES)),) + USE_CXX = true +endif +ifneq ($(strip $(C_SOURCES)),) + USE_C = true +endif + +# the objects files result from the C or C++ sources +SOURCES_DIRECTORIES = $(sort $(dir $(C_SOURCES) $(CXX_SOURCES))) +OBJECTS_LIST = $(patsubst %.c,%.o, $(patsubst %.cc,%.o, $(patsubst %.c++,%.o, $(patsubst %.cpp,%.o, $(notdir $(C_SOURCES) $(CXX_SOURCES)))))) +OBJECTS_EMCC = $(addprefix $(BUILD_GOAL_DIR_EMCC)/,$(patsubst %.c,%.o, $(patsubst %.cc,%.o, $(patsubst %.c++,%.o, $(patsubst %.cpp,%.o, $(notdir $(C_SOURCES) $(CXX_SOURCES))))))) + +# automatic flags settings +ifeq ($(BUILD_TYPE),physics) + USE_ODE = true + EXCLUDE_CONTROLLERS = true +endif +ifeq ($(BUILD_TYPE),controllers) + BUILD_EXECUTABLE = true +else + ifdef STATIC_LIBRARY + BUILD_STATIC_LIBRARY = true + else + BUILD_SHARED_LIBRARY = true + endif +endif + +# target +ifneq ($(C_SOURCES)$(CXX_SOURCES),) + ifdef BUILD_EXECUTABLE + MAIN_TARGET = $(NAME)$(EXE_EXTENSION) + X86_64_LINKER = -target x86_64-apple-macos11 + ARM64_LINKER = -target arm64-apple-macos11 + else + ifdef BUILD_STATIC_LIBRARY + MAIN_TARGET ?= $(LIB_PREFIX)$(NAME)$(STATIC_LIB_EXTENSION) + else + MAIN_TARGET ?= $(LIB_PREFIX)$(NAME)$(SHARED_LIB_EXTENSION) + X86_64_LINKER = -target x86_64-apple-macos11 + ARM64_LINKER = -target arm64-apple-macos11 + endif + endif +endif +ifdef WREN + WREN_TARGET = wrenjs.js +endif + +TARGETS += $(MAIN_TARGET) $(EXTRA_TARGETS) $(WREN_TARGET) + +# goal +SUPPORTED_TARGETS = all release debug profile +ifeq ($(MAKECMDGOALS),) + GOAL = release + GOAL_EMCC = emcc +else + ifdef MAIN_TARGET + ifeq ($(MAKECMDGOALS),$(MAIN_TARGET)) + GOAL_TMP = true + endif + endif + ifeq ($(MAKECMDGOALS),all) + GOAL_TMP = true + endif + + ifdef GOAL_TMP + GOAL = release + else + ifneq ($(filter $(SUPPORTED_TARGETS),$(MAKECMDGOALS)),) + GOAL = $(MAKECMDGOALS) + endif + endif +endif + +# build directory +BUILD_DIR = build +BUILD_GOAL_DIR = $(BUILD_DIR)/$(GOAL) +BUILD_GOAL_DIR_EMCC = $(BUILD_DIR)/$(GOAL_EMCC) + +ifdef GOAL +ifdef FAT_BINARY + $(shell mkdir $(BUILD_DIR) $(BUILD_GOAL_DIR) $(BUILD_GOAL_DIR)/x86_64 $(BUILD_GOAL_DIR)/arm64 2> /dev/null) +else + $(shell mkdir $(BUILD_DIR) $(BUILD_GOAL_DIR) 2> /dev/null) +endif +endif + +ifdef GOAL_EMCC + $(shell mkdir $(BUILD_DIR) $(BUILD_GOAL_DIR_EMCC) 2> /dev/null) +endif + +# files to remove +FILES_TO_REMOVE += $(MAIN_TARGET_COPY) +ifdef TARGET_LIB_DIR + FILES_TO_REMOVE += $(TARGET_LIB_DIR)/$(MAIN_TARGET) +endif + +WRENJS_DIR = $(WEBOTS_HOME_PATH)/resources/web/wwi +###----------------------------------------------------------------------------- +### Compilation flags +###----------------------------------------------------------------------------- + +# global flags +INCLUDE += -I. +WBCFLAGS += -Wall + +# goal dependent flags +ifeq ($(GOAL),debug) + WBCFLAGS += -ggdb +endif + +ifeq ($(TREAT_WARNINGS_AS_ERRORS),1) +CFLAGS += -Werror +endif + +ifeq ($(GOAL),release) + # -O3 has the advantage also to prevents a false positive trojan alert from avast and Kapersky antivirus + WBCFLAGS += -O3 -DNDEBUG + ifneq ($(OSTYPE),darwin) + DYNAMIC_LINK_FLAGS += -s + endif +endif + +ifeq ($(GOAL),profile) + WBCFLAGS += -pg + ifneq ($(OSTYPE),darwin) + ifndef BUILD_STATIC_LIBRARY + LFLAGS += -pg + endif + endif +endif + +# platform dependent flags +ifeq ($(OSTYPE),windows) + WBCFLAGS += -mwindows -Wl,-subsystem,windows -D_GLIBCXX_USE_CXX11_ABI=1 +endif + +ifeq ($(OSTYPE),linux) + DYNAMIC_LIBRARIES += -lm +endif + +ifeq ($(OSTYPE),darwin) +ifdef NO_FAT_BINARY + WBCFLAGS += -target $(PROCESSOR)-apple-macos11 + DYNAMIC_LINK_FLAGS += -target $(PROCESSOR)-apple-macos11 +endif + WBCFLAGS += -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) + DYNAMIC_LINK_FLAGS += -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) +endif + +ifdef USE_CXX + ifdef BUILD_STATIC_LIBRARY + LINKER = ar + else + LINKER = $(CXX) + endif + ifndef EXCLUDE_CONTROLLERS + DYNAMIC_LIBRARIES += -L"$(WEBOTS_CONTROLLER_LIB_PATH)" -lController + ifdef USE_C_API + ifeq ($(OSTYPE),darwin) + INCLUDE += -I"$(WEBOTS_HOME)/Contents/include/controller/c" + else + INCLUDE += -I"$(WEBOTS_HOME)/include/controller/c" + endif + else + DYNAMIC_LIBRARIES += -lCppController + ifeq ($(OSTYPE),darwin) + INCLUDE += -I"$(WEBOTS_HOME)/Contents/include/controller/cpp" + else + INCLUDE += -I"$(WEBOTS_HOME)/include/controller/cpp" + endif + endif + ifeq ($(OSTYPE),windows) + DYNAMIC_LINK_FLAGS += -Wl,--enable-auto-import + endif + endif + ifeq ($(OSTYPE),darwin) + WBCFLAGS += -stdlib=libc++ + DYNAMIC_LINK_FLAGS += -stdlib=libc++ + endif +else + ifdef BUILD_STATIC_LIBRARY + LINKER = ar + else + LINKER = $(CC) + endif + ifndef EXCLUDE_CONTROLLERS + ifeq ($(OSTYPE),darwin) + INCLUDE += -I"$(WEBOTS_HOME)/Contents/include/controller/c" + else + INCLUDE += -I"$(WEBOTS_HOME)/include/controller/c" + endif + DYNAMIC_LIBRARIES += -L"$(WEBOTS_CONTROLLER_LIB_PATH)" -lController + endif +endif + +ifdef BUILD_SHARED_LIBRARY + DYNAMIC_LINK_FLAGS += -shared + ifeq ($(OSTYPE),linux) + WBCFLAGS += -fPIC + endif + ifeq ($(OSTYPE),darwin) + DYNAMIC_LINK_FLAGS += -dynamiclib -compatibility_version 1.0 -current_version 1.0.0 + endif +endif + +ifdef BUILD_STATIC_LIBRARY + ifeq ($(OSTYPE),linux) + WBCFLAGS += -fPIC + endif + STATIC_LINK_FLAGS += rvs +endif + +# Visual Studio +ifeq ($(OSTYPE),windows) + ifdef BUILD_SHARED_LIBRARY + ifndef USE_CXX + ifneq ($(wildcard /C/Program\ Files\ */Microsoft\ Visual\ Studio/2017),) + VISUAL_STUDIO_PATH?=/C/Program Files (x86)/Microsoft Visual Studio/2017 + endif + VS_DEF_NAME = $(NAME).def + endif + endif +endif + +ifeq ($(OSTYPE),darwin) + ifdef WEBOTS_LIBRARY + INSTALL_NAME ?= @rpath/Contents/lib/controller/$(MAIN_TARGET) + LFLAGS += -Xlinker -rpath -Xlinker @loader_path/.. -install_name $(INSTALL_NAME) + else + CALLING_MAKEFILE_DIR := $(shell dirname "$(realpath $(firstword $(MAKEFILE_LIST)))") + WEBOTS_RELATIVE_PATH = $(call computeRelativePath,"$(WEBOTS_HOME_PATH)","$(CALLING_MAKEFILE_DIR)") + ifneq (,$(findstring ..,$(WEBOTS_RELATIVE_PATH))) + ifeq ($(TREAT_WARNINGS_AS_ERRORS),1) # this variable is set only if we do a "make distrib" + DYNAMIC_LINK_FLAGS += -Xlinker -rpath -Xlinker @loader_path/$(WEBOTS_RELATIVE_PATH)../ # add an additional ../ for the Contents folder + else + DYNAMIC_LINK_FLAGS += -Xlinker -rpath -Xlinker @loader_path/$(WEBOTS_RELATIVE_PATH) + endif + ifdef BUILD_SHARED_LIBRARY + INSTALL_NAME ?= @rpath$(subst $(WEBOTS_HOME_PATH),,$(CALLING_MAKEFILE_DIR))/$(MAIN_TARGET) + DYNAMIC_LINK_FLAGS += -install_name $(INSTALL_NAME) + endif + endif + endif +endif + + +###----------------------------------------------------------------------------- +### ODE +###----------------------------------------------------------------------------- + +ifdef USE_ODE + DYNAMIC_LIBRARIES += -L"$(WEBOTS_LIB_PATH)" -lode + ifeq ($(GOAL),profile) + DYNAMIC_LIBRARIES += -lstdc++ + endif + ifeq ($(OSTYPE),darwin) + DYNAMIC_LINK_FLAGS += -flat_namespace -undefined suppress + DYNAMIC_LIBRARIES += "$(WEBOTS_HOME)/Contents/Resources/projects/plugins/physics/physics.a" + INCLUDE += -I"$(WEBOTS_HOME)/Contents/include/ode" -I"$(WEBOTS_HOME)/Contents/include" + else + DYNAMIC_LIBRARIES += "$(WEBOTS_HOME)/resources/projects/plugins/physics/physics.o" + INCLUDE += -I"$(WEBOTS_HOME)/include/ode" -I"$(WEBOTS_HOME)/include" + endif +endif + + +###----------------------------------------------------------------------------- +### Makefile setup +###----------------------------------------------------------------------------- + +# clear out all suffixes for implicit rules to speed up Makefile +.SUFFIXES: +MAKEFLAGS += -r + +# vpath +vpath %.d $(BUILD_DIR) +# find source files +vpath %.c $(SOURCES_DIRECTORIES) +vpath %.cc $(SOURCES_DIRECTORIES) +vpath %.c++ $(SOURCES_DIRECTORIES) +vpath %.cpp $(SOURCES_DIRECTORIES) + +# phony +.PHONY : clean $(SUPPORTED_TARGETS) jar + + +###----------------------------------------------------------------------------- +### Rules +###----------------------------------------------------------------------------- + +$(SUPPORTED_TARGETS): $(TARGETS) + +# we need to be able to execute a make clean inside subdirectories targets (defined as EXTRA_TARGETS) +clean: $(EXTRA_TARGETS) + +### Begin ifdef MAIN_TARGET +ifdef MAIN_TARGET + +MAIN_TARGET_WINDOWS_LIB = $(MAIN_TARGET:.dll=.lib) +ifdef WEBOTS_LIBRARY +MAIN_TARGET_COPY = "$(subst $(space),\ ,$(WEBOTS_CONTROLLER_LIB_PATH))/$(MAIN_TARGET)" +MAIN_TARGET_WINDOWS64_LIB = $(WEBOTS_CONTROLLER_LIB_PATH)/$(MAIN_TARGET_WINDOWS_LIB) +else +MAIN_TARGET_COPY ?= $(MAIN_TARGET) +MAIN_TARGET_WINDOWS64_LIB = $(MAIN_TARGET:.dll=.lib) +endif +ifeq ($(OSTYPE),windows) +FILES_TO_REMOVE += $(MAIN_TARGET_WINDOWS64_LIB) +endif + +###----------------------------------------------------------------------------- +### Rule for the top-level binary +###----------------------------------------------------------------------------- + +ifdef BUILD_STATIC_LIBRARY + LFLAGS += $(STATIC_LINK_FLAGS) + ifndef VERBOSE + AR_SUPPRESS_OUTPUT = > /dev/null 2>&1 + endif +else + LIBRARIES += $(DYNAMIC_LIBRARIES) + LFLAGS += $(DYNAMIC_LINK_FLAGS) -o +endif + +ifdef TARGET_LIB_DIR + +$(MAIN_TARGET): $(TARGET_LIB_DIR)/$(MAIN_TARGET) +ifdef VS_DEF_NAME + @# Generate the .lib libraries to facilitate the integration with Visual Studio + @# if the .def file is existing. + @if [ -f $(VS_DEF_NAME) ]; then \ + if [ -d "$(VISUAL_STUDIO_PATH)" ]; then \ + PATH="$(VISUAL_STUDIO_PATH)/BuildTools/VC/Tools/MSVC/14.16.27023/bin/Hostx64/x64":$PATH lib /machine:X64 /def:$(VS_DEF_NAME) /out:out.lib > /dev/null; \ + mv out.lib $(MAIN_TARGET_WINDOWS64_LIB); \ + rm -f *.exp; \ + else \ + $(ECHO) "\033[0;33m'VISUAL_STUDIO_PATH' environmental variable not set or Microsoft Visual Studio not installed, skipping $(NAME).lib\033[0m"; \ + fi \ + fi +endif + +else + +$(MAIN_TARGET): $(BUILD_GOAL_DIR)/$(MAIN_TARGET) + @# Even when compiled from Webots with possibly the previous version of the binary + @# being executed, it is safe to delete it and copy the new version as a replacement. + @# This new version will be executed only when the process restarts. + @# Note: this binary may be an executable (e.g., a controller) or shared library (e.g., a physics plug-in). + @rm -f $(MAIN_TARGET) > /dev/null 2>&1 && echo "# copying to" $(MAIN_TARGET_COPY) && cp $(BUILD_GOAL_DIR)/$(MAIN_TARGET) $(MAIN_TARGET_COPY) > /dev/null 2>&1 +ifdef VS_DEF_NAME + @# Generate the .lib libraries to facilitate the integration with Visual Studio + @# if the .def file is existing. + @if [ -f $(VS_DEF_NAME) ]; then \ + if [ -d "$(VISUAL_STUDIO_PATH)" ]; then \ + PATH="$(VISUAL_STUDIO_PATH)/BuildTools/VC/Tools/MSVC/14.16.27023/bin/Hostx64/x64":$PATH lib /machine:X64 /def:$(VS_DEF_NAME) /out:out.lib > /dev/null; \ + mv out.lib $(MAIN_TARGET_WINDOWS64_LIB); \ + rm -f *.exp; \ + else \ + $(ECHO) "\033[0;33m'VISUAL_STUDIO_PATH' environmental variable not set or Microsoft Visual Studio not installed, skipping $(NAME).lib\033[0m"; \ + fi \ + fi +endif + +endif + +###----------------------------------------------------------------------------- +### Rule to make the executable file from C/C++ objects +###----------------------------------------------------------------------------- + +ifdef FAT_BINARY +X86_64_OBJECTS = $(addprefix $(BUILD_GOAL_DIR)/x86_64/, $(OBJECTS_LIST)) +ARM64_OBJECTS = $(addprefix $(BUILD_GOAL_DIR)/arm64/, $(OBJECTS_LIST)) + +$(BUILD_GOAL_DIR)/$(MAIN_TARGET): $(BUILD_GOAL_DIR)/x86_64/$(MAIN_TARGET) $(BUILD_GOAL_DIR)/arm64/$(MAIN_TARGET) + @echo "# creating fat" $(notdir $@) + $(SILENT)lipo -create -output $@ $(BUILD_GOAL_DIR)/x86_64/$(MAIN_TARGET) $(BUILD_GOAL_DIR)/arm64/$(MAIN_TARGET) + +$(TARGET_LIB_DIR)/$(MAIN_TARGET): $(BUILD_GOAL_DIR)/x86_64/$(MAIN_TARGET) $(BUILD_GOAL_DIR)/arm64/$(MAIN_TARGET) + @echo "# creating fat" $(notdir $@) + $(SILENT)lipo -create -output $@ $(BUILD_GOAL_DIR)/x86_64/$(MAIN_TARGET) $(BUILD_GOAL_DIR)/arm64/$(MAIN_TARGET) + +$(BUILD_GOAL_DIR)/x86_64/$(MAIN_TARGET): $(X86_64_OBJECTS) $(LINK_DEPENDENCIES) + @echo "# linking" $(notdir $@) "(x86_64)" + $(SILENT)$(LINKER) $(X86_64_LINKER) $(LFLAGS) $@ $(X86_64_OBJECTS) $(LIBRARIES) $(AR_SUPPRESS_OUTPUT) + +$(BUILD_GOAL_DIR)/arm64/$(MAIN_TARGET): $(ARM64_OBJECTS) $(LINK_DEPENDENCIES) + @echo "# linking" $(notdir $@) "(arm64)" + $(SILENT)$(LINKER) $(ARM64_LINKER) $(LFLAGS) $@ $(ARM64_OBJECTS) $(LIBRARIES) $(AR_SUPPRESS_OUTPUT) + +else + +OBJECTS = $(addprefix $(BUILD_GOAL_DIR)/, $(OBJECTS_LIST)) +$(BUILD_GOAL_DIR)/$(MAIN_TARGET): $(OBJECTS) $(LINK_DEPENDENCIES) + @echo "# linking" $(notdir $@) + $(SILENT)$(LINKER) $(LFLAGS) $@ $(OBJECTS) $(LIBRARIES) $(AR_SUPPRESS_OUTPUT) + +$(TARGET_LIB_DIR)/$(MAIN_TARGET): $(OBJECTS) $(LINK_DEPENDENCIES) + @echo "# linking" $(notdir $@) + $(SILENT)$(LINKER) $(LFLAGS) $@ $(OBJECTS) $(LIBRARIES) $(AR_SUPPRESS_OUTPUT) + +endif + +endif + +### End ifdef MAIN_TARGET + +ifdef WREN +$(WREN_TARGET): $(OBJECTS_EMCC) + $(SILENT) emcc -O3 -s WASM=1 -s EXIT_RUNTIME=1 -s USE_WEBGL2=1 -s FULL_ES3=1 -s ALLOW_MEMORY_GROWTH=1 --preload-file ../../resources/wren/shaders@../../resources/wren/shaders $(OBJECTS_EMCC) -s EXPORTED_FUNCTIONS='[$(shell cat functions_to_export.txt)]' -s 'EXPORTED_RUNTIME_METHODS=["ccall", "getValue"]' -o wrenjs.js + $(SILENT) cp wrenjs.js wrenjs.wasm wrenjs.data $(WRENJS_DIR)/ > /dev/null 2>&1 +endif + +###----------------------------------------------------------------------------- +### Rules to make automatic dependencies adapted from the GNU make info file +###----------------------------------------------------------------------------- +$(BUILD_GOAL_DIR)/%.d:%.c + @echo "# updating" $(notdir $@) + $(SILENT)$(CC) $(INCLUDE) $(WBCFLAGS) $(CFLAGS) -MM $< -MT $(addprefix $(BUILD_GOAL_DIR)/,$(notdir $(<:.c=.o))) > $@ + +$(BUILD_GOAL_DIR)/%.d:%.cc + @echo "# updating" $(notdir $@) + $(SILENT)$(CXX) $(INCLUDE) $(WBCFLAGS) $(CFLAGS -MM $< -MT $(addprefix $(BUILD_GOAL_DIR)/,$(notdir $(<:.cc=.o))) > $@ + +$(BUILD_GOAL_DIR)/%.d:%.c++ + @echo "# updating" $(notdir $@) + $(SILENT)$(CXX) $(INCLUDE) $(WBCFLAGS) $(CFLAGS) -MM $< -MT $(addprefix $(BUILD_GOAL_DIR)/,$(notdir $(<:.c++=.o))) > $@ + +$(BUILD_GOAL_DIR)/%.d:%.cpp + @echo "# updating" $(notdir $@) + $(SILENT)$(CXX) $(INCLUDE) $(WBCFLAGS) $(CFLAGS) -MM $< -MT $(addprefix $(BUILD_GOAL_DIR)/,$(notdir $(<:.cpp=.o))) > $@ + +# dependencies +ifdef GOAL + DEPENDENCIES = $(addprefix $(BUILD_GOAL_DIR)/,$(notdir $(C_SOURCES:.c=.d) $(CXX_SOURCES:.cpp=.d))) + ifneq ($(DEPENDENCIES),) + -include $(DEPENDENCIES) + endif +endif + +###----------------------------------------------------------------------------- +### Generic rules to make the object files +###----------------------------------------------------------------------------- +$(BUILD_GOAL_DIR)/%.o:%.c + @echo "# compiling" $(notdir $<) + $(SILENT)$(CC) -c $(WBCFLAGS) $(CFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/x86_64/%.o:%.c + @echo "# compiling" $(notdir $<) "(x86_64)" + $(SILENT)$(CC) -c -target x86_64-apple-macos11 $(WBCFLAGS) $(CFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/arm64/%.o:%.c + @echo "# compiling" $(notdir $<) "(arm64)" + $(SILENT)$(CC) -c -target arm64-apple-macos11 $(WBCFLAGS) $(CFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/%.o:%.cc + @echo "# compiling" $(notdir $<) + $(SILENT)$(CXX) -c $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/%.o:%.c++ + @echo "# compiling" $(notdir $<) + $(SILENT)$(CXX) -c $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/%.o:%.cpp + @echo "# compiling" $(notdir $<) + $(SILENT)$(CXX) -c $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/x86_64/%.o:%.cpp + @echo "# compiling" $(notdir $<) "(x86_64)" + $(SILENT)$(CXX) -c -target x86_64-apple-macos11 $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR)/arm64/%.o:%.cpp + @echo "# compiling" $(notdir $<) "(arm64)" + $(SILENT)$(CXX) -c -target arm64-apple-macos11 $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $@ + +$(BUILD_GOAL_DIR_EMCC)/%.o:%.cpp + @echo "# compiling with emcc " $(notdir $<) + $(SILENT)emcc -O3 -c $(WBCFLAGS) $(CFLAGS) $(CXXFLAGS) $(INCLUDE) $< -o $(@) + +###----------------------------------------------------------------------------- +### How to clean up the directory +###----------------------------------------------------------------------------- +clean: + $(SILENT)rm -fr $(FILES_TO_REMOVE) $(BUILD_DIR) > /dev/null 2>&1 || : + $(SILENT)rm -d $(EMPTY_DIRECTORIES_TO_REMOVE) > /dev/null 2>&1 || : + ifdef WREN + $(SILENT)rm $(WEBOTS_HOME_PATH)/src/wren/wrenjs.js $(WEBOTS_HOME_PATH)/src/wren/wrenjs.data $(WEBOTS_HOME_PATH)/src/wren/wrenjs.wasm $(WEBOTS_HOME_PATH)/src/wren/functions_to_export.txt > /dev/null 2>&1 || : + $(SILENT)rm $(WRENJS_DIR)/wrenjs.js $(WRENJS_DIR)/wrenjs.data $(WRENJS_DIR)/wrenjs.wasm > /dev/null 2>&1 || : + endif + $(SILENT)rm -r $(WRENJS_DIR)/images/post_processing > /dev/null 2>&1 || : diff --git a/webots_ros2_driver/webots/resources/Makefile.os.include b/webots_ros2_driver/webots/resources/Makefile.os.include new file mode 100644 index 000000000..35d05e89c --- /dev/null +++ b/webots_ros2_driver/webots/resources/Makefile.os.include @@ -0,0 +1,110 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# determine the OSTYPE + +ifeq ($(OS),Windows_NT) + OSTYPE = windows +endif + +ifndef OSTYPE + OSTYPE := $(shell uname) +endif + +ifeq ($(OSTYPE),Linux) + OSTYPE = linux +endif + +ifeq ($(OSTYPE),linux-gnu) + OSTYPE = linux +endif + +ifeq ($(OSTYPE),) + OSTYPE = windows +endif + +ifeq ($(OSTYPE),msys) + OSTYPE = windows +endif + +ifneq ($(findstring MINGW,$(OSTYPE)),) + OSTYPE = windows +endif + +ifneq ($(findstring CYGWIN,$(OSTYPE)),) + OSTYPE = windows +endif + +ifeq ($(OSTYPE),Darwin) + OSTYPE = darwin +endif + +# determine the OSARCH and UBUNTU_VERSION (linux only) +ifeq ($(OSTYPE),linux) + ifeq ($(filter x86_64, $(shell uname -a)),) + OSARCH = i386 + else + OSARCH = x86_64 + endif + ifeq ($(OSTYPE),linux) + ifeq ($(SNAP_NAME), webots) + UBUNTU_VERSION=20.04 + else + ifneq (, $(shell which lsb_release)) + UBUNTU_VERSION:=$(shell lsb_release -sr) + endif + endif + endif +endif + +# OS specific variables + +ifeq ($(OSTYPE),darwin) + + SUPPORTED_SDK = 11 12 + MACOSX_MIN_SDK_VERSION = 11 + ECHO = echo + # cf. https://stackoverflow.com/a/30225756/2210777 + MACOSX_SDK_PATH = $(shell xcrun --sdk macosx --show-sdk-path) + ifeq ($(wildcard $(MACOSX_SDK_PATH)),) + $(error The macOS SDK is not found. Please check that XCode and its command line tools are correctly installed.) + endif + MACOSX_SDK_VERSION = $(shell egrep -o '10.[0-9][0-9]' <<< $(MACOSX_SDK_PATH) ) + ifneq ($(MACOSX_SDK_VERSION),$(filter $(MACOSX_SDK_VERSION),$(SUPPORTED_SDK))) + ## Only recent SDKs are working, but this doesn't prevent to use others. + $(warning Your macOS SDK ("$(MACOSX_SDK_PATH)") is $(MACOSX_SDK_VERSION) while Webots supports the $(SUPPORTED_SDK) SDKs.) + endif +else + ECHO = echo -e +endif + +# extensions and prefixes +STATIC_LIB_EXTENSION = .a +ifeq ($(OSTYPE),windows) + EXE_EXTENSION = .exe + SHARED_LIB_EXTENSION = .dll + WEBOTS_LIB_PATH=$(subst \,/,$(WEBOTS_HOME))/msys64/mingw64/bin + WEBOTS_CONTROLLER_LIB_PATH=$(subst \,/,$(WEBOTS_HOME))/lib/controller +else + LIB_PREFIX = lib + ifeq ($(OSTYPE),linux) + SHARED_LIB_EXTENSION = .so + WEBOTS_LIB_PATH=$(WEBOTS_HOME)/lib/webots + WEBOTS_CONTROLLER_LIB_PATH=$(WEBOTS_HOME)/lib/controller + else # macOS + SHARED_LIB_EXTENSION = .dylib + WEBOTS_LIB_PATH=$(WEBOTS_HOME)/Contents/lib/webots + WEBOTS_CONTROLLER_LIB_PATH=$(WEBOTS_HOME)/Contents/lib/controller + endif +endif diff --git a/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/Makefile b/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/Makefile new file mode 100644 index 000000000..951c7c662 --- /dev/null +++ b/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/Makefile @@ -0,0 +1,26 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Webots Makefile system +# +# You may add some variable definitions hereafter to customize the build process +# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include + +WEBOTS_LIBRARY = 1 +C_SOURCES = $(wildcard *.c) + +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +include $(WEBOTS_HOME_PATH)/resources/Makefile.include diff --git a/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/generic.c b/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/generic.c new file mode 100644 index 000000000..470a99a9b --- /dev/null +++ b/webots_ros2_driver/webots/resources/projects/libraries/generic_robot_window/generic.c @@ -0,0 +1,280 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +static bool configured = false; +static int time_step = 0; +static bool is_hidden = false; +static double refresh_rate = 0.032; // s +static double last_update_time = 0; // s +static bool isDeviceTypeControlEnabled[WB_NODE_TOUCH_SENSOR - WB_NODE_ACCELEROMETER]; + +static WbNodeType stringToDeviceType(const char *typeString) { + if (strcmp(typeString, "Accelerometer") == 0) + return WB_NODE_ACCELEROMETER; + if (strcmp(typeString, "Brake") == 0) + return WB_NODE_BRAKE; + if (strcmp(typeString, "Camera") == 0) + return WB_NODE_CAMERA; + if (strcmp(typeString, "Compass") == 0) + return WB_NODE_COMPASS; + if (strcmp(typeString, "Connector") == 0) + return WB_NODE_CONNECTOR; + if (strcmp(typeString, "Display") == 0) + return WB_NODE_DISPLAY; + if (strcmp(typeString, "DistanceSensor") == 0) + return WB_NODE_DISTANCE_SENSOR; + if (strcmp(typeString, "Emitter") == 0) + return WB_NODE_EMITTER; + if (strcmp(typeString, "GPS") == 0) + return WB_NODE_GPS; + if (strcmp(typeString, "Gyro") == 0) + return WB_NODE_GYRO; + if (strcmp(typeString, "InertialUnit") == 0) + return WB_NODE_INERTIAL_UNIT; + if (strcmp(typeString, "LED") == 0) + return WB_NODE_LED; + if (strcmp(typeString, "Lidar") == 0) + return WB_NODE_LIDAR; + if (strcmp(typeString, "LightSensor") == 0) + return WB_NODE_LIGHT_SENSOR; + if (strcmp(typeString, "LinearMotor") == 0) + return WB_NODE_LINEAR_MOTOR; + if (strcmp(typeString, "Pen") == 0) + return WB_NODE_PEN; + if (strcmp(typeString, "PositionSensor") == 0) + return WB_NODE_POSITION_SENSOR; + if (strcmp(typeString, "Propeller") == 0) + return WB_NODE_PROPELLER; + if (strcmp(typeString, "Radar") == 0) + return WB_NODE_RADAR; + if (strcmp(typeString, "RangeFinder") == 0) + return WB_NODE_RANGE_FINDER; + if (strcmp(typeString, "Receiver") == 0) + return WB_NODE_RECEIVER; + if (strcmp(typeString, "RotationalMotor") == 0) + return WB_NODE_ROTATIONAL_MOTOR; + if (strcmp(typeString, "Speaker") == 0) + return WB_NODE_SPEAKER; + if (strcmp(typeString, "TouchSensor") == 0) + return WB_NODE_TOUCH_SENSOR; + return WB_NODE_NO_NODE; +} + +static void enable_device(WbDeviceTag tag, bool enable) { + const WbNodeType type = wb_device_get_node_type(tag); + if (!isDeviceTypeControlEnabled[type - WB_NODE_ACCELEROMETER]) + return; + + const int enableRate = enable ? time_step : 0; + switch (type) { + case WB_NODE_ACCELEROMETER: + wb_accelerometer_enable(tag, enableRate); + break; + case WB_NODE_CAMERA: + wb_camera_enable(tag, enableRate); + break; + case WB_NODE_COMPASS: + wb_compass_enable(tag, enableRate); + break; + case WB_NODE_DISTANCE_SENSOR: + wb_distance_sensor_enable(tag, enableRate); + break; + case WB_NODE_GPS: + wb_gps_enable(tag, enableRate); + break; + case WB_NODE_GYRO: + wb_gyro_enable(tag, enableRate); + break; + case WB_NODE_INERTIAL_UNIT: + wb_inertial_unit_enable(tag, enableRate); + break; + case WB_NODE_LIDAR: + wb_lidar_enable(tag, enableRate); + break; + case WB_NODE_LIGHT_SENSOR: + wb_light_sensor_enable(tag, enableRate); + break; + case WB_NODE_POSITION_SENSOR: + wb_position_sensor_enable(tag, enableRate); + break; + case WB_NODE_RADAR: + wb_radar_enable(tag, enableRate); + break; + case WB_NODE_RANGE_FINDER: + wb_range_finder_enable(tag, enableRate); + break; + case WB_NODE_TOUCH_SENSOR: + wb_touch_sensor_enable(tag, enableRate); + break; + default: + assert(0); + } +} + +// JavaScript -> C protocol description: +// [deviceName:commandTag[=commadState][,]] +// example: +// "e-puck:forward", "ds0:enable" or "myMotor0:value=1.2" +void wbu_generic_robot_window_parse_device_command(char *token, char *tokens) { + WbDeviceTag tag = 0; + bool robot = false; + + while (token && token[0] != '\0') { + if (tag == 0 && !robot) { // first token = device or robot name + char *name0 = wbu_string_replace(token, "\\:", ":"); + char *name = wbu_string_replace(name0, "\\,", ","); + if (strcmp(name, wb_robot_get_name()) == 0) + robot = true; + else + tag = wb_robot_get_device(name); + free(name); + free(name0); + } else if (strcmp(token, "enable") == 0) + enable_device(tag, true); + else if (strcmp(token, "disable") == 0) + enable_device(tag, false); + else if (strncmp(token, "position=", 9) == 0 && tag > 0) { + double position = atof(&token[9]); + wb_motor_set_position(tag, position); + } else if (strcmp(token, "recognitionEnable") == 0) + wb_camera_recognition_enable(tag, time_step); + else if (strcmp(token, "recognitionDisable") == 0) + wb_camera_recognition_disable(tag); + else if (strcmp(token, "segmentationEnable") == 0) + wb_camera_recognition_enable_segmentation(tag); + else if (strcmp(token, "segmentationDisable") == 0) + wb_camera_recognition_disable_segmentation(tag); + else if (strcmp(token, "pointCloudEnable") == 0) + wb_lidar_enable_point_cloud(tag); + else if (strcmp(token, "pointCloudDisable") == 0) + wb_lidar_disable_point_cloud(tag); + else + assert(0); // protocol issue + + token = wbu_string_strsep(&tokens, ":"); + }; +} + +bool wbu_generic_robot_window_parse_device_control_command(char *first_token, char *tokens) { + if (strcmp(first_token, "device-control-mode") != 0) + return false; + WbNodeType type = WB_NODE_NO_NODE; + + char *token = NULL; + while ((token = wbu_string_strsep(&tokens, ":"))) { + if (type == WB_NODE_NO_NODE) { + type = stringToDeviceType(token); + if (type == WB_NODE_NO_NODE) { + assert(0); // protocol issue + return false; + } + } else { + if (strcmp(token, "1") == 0) { + isDeviceTypeControlEnabled[type - WB_NODE_ACCELEROMETER] = true; + return true; + } else if (strcmp(token, "0") == 0) { + isDeviceTypeControlEnabled[type - WB_NODE_ACCELEROMETER] = false; + return true; + } + } + } + assert(0); // protocol issue + return false; +} + +void wbu_generic_robot_window_init() { + time_step = (int)wb_robot_get_basic_time_step(); + + const int device_size = WB_NODE_TOUCH_SENSOR - WB_NODE_ACCELEROMETER; + for (int i = 0; i < device_size; ++i) + isDeviceTypeControlEnabled[i] = false; +} + +bool wbu_generic_robot_window_handle_messages(const char *message) { + if (strncmp(message, "configure", 9) == 0) { + int max_image_height = -1; + int max_image_width = -1; + int hidden = 1; + if (sscanf(message, "configure { \"imageMaxWidth\": %d, \"imageMaxHeight\": %d, \"hidden\": %d }", &max_image_width, + &max_image_height, &hidden) != 3) { + fprintf(stderr, "Wrong 'configure' message received from the robot window.\n"); + assert(0); + return false; + } + is_hidden = hidden == 0 ? false : true; + wbu_default_robot_window_set_images_max_size(max_image_width, max_image_height); + wbu_default_robot_window_configure(); + configured = true; + return true; + } else if (strncmp(message, "refresh-rate", 12) == 0) { + int rate = -1; + if (sscanf(message, "refresh-rate %d", &rate) != 1) { + fprintf(stderr, "Wrong 'refresh-rate' message received from the robot window.\n"); + assert(0); + return false; + } + refresh_rate = 0.001 * rate; + return true; + } else if (strncmp(message, "window", 6) == 0) { + is_hidden = strstr(message, "hidden") != NULL; + return true; + } + return false; +} + +void wbu_generic_robot_window_update() { + last_update_time = wb_robot_get_time(); + wbu_default_robot_window_update(); +} + +bool wbu_generic_robot_window_is_hidden() { + return is_hidden; +} + +double wbu_generic_robot_window_refresh_rate() { + return refresh_rate; +} + +bool wbu_generic_robot_window_needs_update() { + return configured && !is_hidden && (refresh_rate == 0 || (wb_robot_get_time() - last_update_time) > refresh_rate); +} diff --git a/webots_ros2_driver/webots/src/controller/Makefile b/webots_ros2_driver/webots/src/controller/Makefile new file mode 100644 index 000000000..343c7b33f --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/Makefile @@ -0,0 +1,34 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +include ../../resources/Makefile.os.include + +.PHONY: release debug profile + +release debug profile clean: + @echo "#" + @echo "# C controller library ("$@")" + @echo "#" + @+make -s -C c $@ + @echo "#" + @echo "# C++ controller library ("$@")" + @echo "#" + @+make -s -C cpp $@ + @echo "#" + @echo "# Java controller library ("$@")" + @echo "#" + @+make -s -C java $@ + @echo "# Matlab controller library ("$@")" + @echo "#" + @+make -s -C matlab $@ diff --git a/webots_ros2_driver/webots/src/controller/c/Controller.def b/webots_ros2_driver/webots/src/controller/c/Controller.def new file mode 100644 index 000000000..a7ee664bf --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/Controller.def @@ -0,0 +1,624 @@ +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_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_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_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_pose_tracking +wb_supervisor_node_enable_contact_point_tracking +wb_supervisor_node_enable_pose_tracking +wb_supervisor_node_export_string +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_fields +wb_supervisor_node_get_number_of_contact_points +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_field +wb_supervisor_node_get_proto_field_by_index +wb_supervisor_node_get_proto_number_of_fields +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_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 +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/webots_ros2_driver/webots/src/controller/c/Makefile b/webots_ros2_driver/webots/src/controller/c/Makefile new file mode 100644 index 000000000..739e82050 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/Makefile @@ -0,0 +1,189 @@ +# Copyright 1996-2022 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +ifeq ($(WEBOTS_HOME),) +WEBOTS_HOME_PATH = ../../.. +else +null := +space := $(null) $(null) +WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) +endif + +default: + @echo "Type one of the following (OSTYPE is \""$(OSTYPE)"\"):" + @echo " make debug for debug (with gdb symbols)" + @echo " make profile for profiling (with gprof information)" + @echo " make release for finale release" + @echo " make clean remove build directory" + @echo " make count-lines count the lines of code of C controller library" + +count-lines: + wc -l *.c* *.h* + +include $(WEBOTS_HOME_PATH)/resources/Makefile.include + +WEBOTS_DEPENDENCY_PATH ?= $(WEBOTS_HOME_PATH)/dependencies + +ifeq ($(OSTYPE),darwin) +CFLAGS = -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -fpascal-strings -Wno-deprecated-declarations -I/Developer/Headers/FlatCarbon -Wall -fno-common +INCLUDE = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include +LD_FLAGS = -mmacosx-version-min=$(MACOSX_MIN_SDK_VERSION) -dynamiclib -install_name @rpath/Contents/lib/controller/libController.dylib -compatibility_version 1.0 -current_version 1.0.0 +SHARED_LIBS = -lz +TARGET = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.dylib +CC = gcc +endif + +ifeq ($(OSTYPE),linux) +CFLAGS = -fPIC -Wall -Wno-stringop-truncation +INCLUDE = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include +LD_FLAGS = -shared -Wl,-soname,libController.so +SHARED_LIBS = -lm -lpthread -ldl -lrt +TARGET = $(WEBOTS_CONTROLLER_LIB_PATH)/libController.so +endif + +ifeq ($(OSTYPE),windows) +CFLAGS = -Wall -mwindows -frandom-seed=0 +INCLUDE = -I$(WEBOTS_HOME_PATH)/include/controller/c -I$(WEBOTS_HOME_PATH)/include -I/mingw64/include +LD_FLAGS = -shared -mwindows --def Controller.def -lws2_32 +PLATFORM = win32 +CC = gcc +TARGET = $(WEBOTS_CONTROLLER_LIB_PATH)/Controller.dll $(WEBOTS_CONTROLLER_LIB_PATH)/Controller.lib +endif + +ifeq ($(MAKECMDGOALS),debug) +CFLAGS += -g +endif + +ifeq ($(TREAT_WARNINGS_AS_ERRORS),1) +CFLAGS += -Werror +endif + +ifeq ($(MAKECMDGOALS),profile) +CFLAGS += -pg -O3 -DNDEBUG +LD_FLAGS += -pg +endif + +OBJDIR = build/$(MAKECMDGOALS) +ifneq (,$(findstring $(MAKECMDGOALS),debug profile release)) +BUILD_PATH_SETUP := $(shell mkdir -p $(OBJDIR)) +ifeq ($(OSTYPE),darwin) +BUILD_PATH_SETUP := $(shell mkdir -p $(OBJDIR)/x86_64 $(OBJDIR)/arm64) +endif +endif + +ifdef EXTCONTROLLER +CFLAGS += -DEXTCONTROLLER +endif + +ifeq ($(MAKECMDGOALS),release) +CFLAGS += -O3 -DNDEBUG +ifneq ($(OSTYPE),darwin) +LD_FLAGS += -s +endif +endif + +vpath %.o $(OBJDIR) +vpath %.c . + +INCLUDE += -isystem $(WEBOTS_HOME_PATH)/src/stb + +SOURCES=$(shell ls *.c) + +OBJECTS=$(notdir $(SOURCES:.c=.o)) + +# clear out all suffixes for implicit rules to speed up Makefile +.SUFFIXES: +%:: %,v +%:: RCS/%,v +%:: RCS/% +%:: s.% +%:: SCCS/s.% + +ifneq (,$(filter $(MAKECMDGOALS),debug release profile)) # if $(MAKECMDGOALS) is either of those +-include $(addprefix $(OBJDIR)/,$(notdir $(SOURCES:.c=.d))) +endif + +debug profile release: $(TARGET) + +ifeq ($(OSTYPE),windows) + +MINGW64_OBJECTS = $(addprefix $(OBJDIR)/, $(OBJECTS)) + +$(WEBOTS_CONTROLLER_LIB_PATH)/Controller.dll: $(MINGW64_OBJECTS) Controller.def + @echo "# linking "$@ + @echo "# linking "$(WEBOTS_CONTROLLER_LIB_PATH)"/libController.a" + @$(CC) -o $@ $(addprefix $(OBJDIR)/, $(OBJECTS)) $(LD_FLAGS) -Wl,--out-implib,$(WEBOTS_CONTROLLER_LIB_PATH)/libController.a + +$(WEBOTS_CONTROLLER_LIB_PATH)/Controller.lib: $(WEBOTS_CONTROLLER_LIB_PATH)/Controller.dll Controller.def +ifeq ($(VISUAL_STUDIO_PATH),) + @$(ECHO) "# \033[0;33m'VISUAL_STUDIO_PATH' not set, skipping Controller.lib\033[0m" +else + @echo "# creating "$@ + @PATH="$(VISUAL_STUDIO_PATH)/BuildTools/VC/Tools/MSVC/14.16.27023/bin/Hostx64/x64":$PATH lib /machine:X64 /def:Controller.def /out:Controller.lib > /dev/null + @mv Controller.lib "$(WEBOTS_CONTROLLER_LIB_PATH)" + @rm Controller.exp +endif + +endif + +ifeq ($(OSTYPE),darwin) +X86_64_OBJECTS = $(addprefix $(OBJDIR)/x86_64/, $(OBJECTS)) +ARM64_OBJECTS = $(addprefix $(OBJDIR)/arm64/, $(OBJECTS)) + +$(TARGET): $(OBJDIR)/x86_64/libController.dylib $(OBJDIR)/arm64/libController.dylib + @echo "# creating fat binary: $$"\(WEBOTS_HOME\)/lib/controller/libController.dylib + @lipo -create -output $(TARGET) $(OBJDIR)/x86_64/libController.dylib $(OBJDIR)/arm64/libController.dylib + @chmod a-x $@ + @codesign -s - $@ + +$(OBJDIR)/x86_64/libController.dylib: $(X86_64_OBJECTS) + @echo "# linking "$@" (x86_64)" + @$(CC) $(LD_FLAGS) -target x86_64-apple-macos11 -o $@ $(X86_64_OBJECTS) $(SHARED_LIBS) + @chmod a-x $@ + +$(OBJDIR)/arm64/libController.dylib: $(ARM64_OBJECTS) + @echo "# linking "$@" (arm64)" + @$(CC) $(LD_FLAGS) -target arm64-apple-macos11 -o $@ $(ARM64_OBJECTS) $(SHARED_LIBS) + @chmod a-x $@ + +endif + +ifeq ($(OSTYPE),linux) +$(TARGET): $(OBJECTS) + @echo "# linking "$@ + @$(CC) $(LD_FLAGS) -o $(TARGET) $(addprefix $(OBJDIR)/, $(OBJECTS)) $(SHARED_LIBS) + @chmod a-x $(TARGET) +endif + +$(OBJDIR)/%.o:%.c + @echo "# compiling "$< + @$(CC) -c $(CFLAGS) $(EXTRA_CFLAGS) $(INCLUDE) $< -o $@ + +$(OBJDIR)/arm64/%o:%c + @echo "# compiling "$<" (arm64)" + @$(CC) -c -target arm64-apple-macos11 $(CFLAGS) $(EXTRA_CFLAGS) $(INCLUDE) $< -o $(OBJDIR)/arm64/$(notdir $@) + +$(OBJDIR)/x86_64/%o:%c + @echo "# compiling "$<" (x86_64)" + @$(CC) -c -target x86_64-apple-macos11 $(CFLAGS) $(EXTRA_CFLAGS) $(INCLUDE) $< -o $(OBJDIR)/x86_64/$(notdir $@) + +$(OBJDIR)/%.d:%.c + @echo "# updating "$(notdir $@) + @echo $(basename $(OBJDIR)/$(notdir $@)).o: $(basename $@).d > $(OBJDIR)/$(notdir $@) + @printf "%s" "$(OBJDIR)/" >> $(OBJDIR)/$(notdir $@) + @$(CC) $(CFLAGS) -MM -w $(INCLUDE) $< >> $(OBJDIR)/$(notdir $@) + +FILES_TO_REMOVE = $(TARGET) +FILES_TO_REMOVE += $(WEBOTS_CONTROLLER_LIB_PATH)/libController.a +FILES_TO_REMOVE += $(WEBOTS_CONTROLLER_LIB_PATH)/libController.lib diff --git a/webots_ros2_driver/webots/src/controller/c/README.TXT b/webots_ros2_driver/webots/src/controller/c/README.TXT new file mode 100644 index 000000000..a026e9135 --- /dev/null +++ b/webots_ros2_driver/webots/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/webots_ros2_driver/webots/src/controller/c/abstract_camera.c b/webots_ros2_driver/webots/src/controller/c/abstract_camera.c new file mode 100644 index 000000000..f648218e9 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/abstract_camera.c @@ -0,0 +1,171 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "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(WbDevice *d) { + int sampling_period = 0; + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + if (ac) + sampling_period = ac->sampling_period; + robot_mutex_unlock(); + return sampling_period; +} + +int wb_abstract_camera_get_height(WbDevice *d) { + int result = -1; + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + if (ac) + result = ac->height; + robot_mutex_unlock(); + return result; +} + +int wb_abstract_camera_get_width(WbDevice *d) { + int result = -1; + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + if (ac) + result = ac->width; + robot_mutex_unlock(); + return result; +} + +double wb_abstract_camera_get_fov(WbDevice *d) { + double result = NAN; + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + if (ac) + result = ac->fov; + robot_mutex_unlock(); + return result; +} + +double wb_abstract_camera_get_near(WbDevice *d) { + double result = NAN; + robot_mutex_lock(); + AbstractCamera *ac = d->pdata; + if (ac) + result = ac->camnear; + robot_mutex_unlock(); + return result; +} diff --git a/webots_ros2_driver/webots/src/controller/c/abstract_camera.h b/webots_ros2_driver/webots/src/controller/c/abstract_camera.h new file mode 100644 index 000000000..bf459f2c7 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/abstract_camera.h @@ -0,0 +1,64 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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(WbDevice *d); +int wb_abstract_camera_get_height(WbDevice *d); +int wb_abstract_camera_get_width(WbDevice *d); +double wb_abstract_camera_get_fov(WbDevice *d); +double wb_abstract_camera_get_near(WbDevice *d); + +#endif // ABSTRACT_CAMERA_PRIVATE_H diff --git a/webots_ros2_driver/webots/src/controller/c/accelerometer.c b/webots_ros2_driver/webots/src/controller/c/accelerometer.c new file mode 100644 index 000000000..460798921 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/accelerometer.c @@ -0,0 +1,191 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include // 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(); + 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) { + double *result = NULL; + robot_mutex_lock(); + 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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/altimeter.c b/webots_ros2_driver/webots/src/controller/c/altimeter.c new file mode 100644 index 000000000..503a67478 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/altimeter.c @@ -0,0 +1,144 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include "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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/ansi_codes.c b/webots_ros2_driver/webots/src/controller/c/ansi_codes.c new file mode 100644 index 000000000..7da70a823 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/ansi_codes.c @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +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/webots_ros2_driver/webots/src/controller/c/base64.c b/webots_ros2_driver/webots/src/controller/c/base64.c new file mode 100644 index 000000000..6c3f45a40 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/base64.c @@ -0,0 +1,59 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// 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/webots_ros2_driver/webots/src/controller/c/base64.h b/webots_ros2_driver/webots/src/controller/c/base64.h new file mode 100644 index 000000000..15164c2bc --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/base64.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/brake.c b/webots_ros2_driver/webots/src/controller/c/brake.c new file mode 100644 index 000000000..1f6e06169 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/brake.c @@ -0,0 +1,164 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +//*************************************************************************** +// 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; + } + + 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(); + 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/webots_ros2_driver/webots/src/controller/c/camera.c b/webots_ros2_driver/webots/src/controller/c/camera.c new file mode 100644 index 000000000..dd3aa3e13 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/camera.c @@ -0,0 +1,893 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include "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->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) { + 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) { + 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) { + 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) { + 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) { + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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) { + WbDevice *d = camera_get_device(tag); + if (!d) + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + + return wb_abstract_camera_get_near(camera_get_device(tag)); +} + +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(); + 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(); + 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(); + 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(); + 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(); + 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) { + 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(); + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/camera_private.h b/webots_ros2_driver/webots/src/controller/c/camera_private.h new file mode 100644 index 000000000..c01f3b0e3 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/camera_private.h @@ -0,0 +1,48 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/compass.c b/webots_ros2_driver/webots/src/controller/c/compass.c new file mode 100644 index 000000000..ed4c3eb36 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/compass.c @@ -0,0 +1,190 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include "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(); + 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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/connector.c b/webots_ros2_driver/webots/src/controller/c/connector.c new file mode 100644 index 000000000..e3c919555 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/connector.c @@ -0,0 +1,187 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +//*************************************************************************** +// 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) { + 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(); + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/console.c b/webots_ros2_driver/webots/src/controller/c/console.c new file mode 100644 index 000000000..c29f4161e --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/console.c @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include + +#include "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/webots_ros2_driver/webots/src/controller/c/default_robot_window.c b/webots_ros2_driver/webots/src/controller/c/default_robot_window.c new file mode 100644 index 000000000..02abbcf19 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/default_robot_window.c @@ -0,0 +1,871 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// 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 "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: + 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(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 { + 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); + } +} + +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; + 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; + 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/webots_ros2_driver/webots/src/controller/c/default_robot_window_private.h b/webots_ros2_driver/webots/src/controller/c/default_robot_window_private.h new file mode 100644 index 000000000..883fac59c --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/default_robot_window_private.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef DEFAULT_ROBOT_WINDOW_PRIVATE_H +#define DEFAULT_ROBOT_WINDOW_PRIVATE_H + +void default_robot_window_cleanup(); + +#endif diff --git a/webots_ros2_driver/webots/src/controller/c/device.c b/webots_ros2_driver/webots/src/controller/c/device.c new file mode 100644 index 000000000..c689d3b81 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/device.c @@ -0,0 +1,167 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include "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_touch_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 *); + +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_ROTATIONAL_MOTOR: + wb_motor_init(d); + break; + case WB_NODE_TOUCH_SENSOR: + wb_touch_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_SKIN: + wb_skin_init(d); + break; + case WB_NODE_SPEAKER: + wb_speaker_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/webots_ros2_driver/webots/src/controller/c/device_private.h b/webots_ros2_driver/webots/src/controller/c/device_private.h new file mode 100644 index 000000000..47a3c0b0a --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/device_private.h @@ -0,0 +1,43 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/display.c b/webots_ros2_driver/webots/src/controller/c/display.c new file mode 100644 index 000000000..e9f58424c --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/display.c @@ -0,0 +1,887 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include "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) { + 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(); + 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(); + 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) { + 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) { + 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) { + 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(); + 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); + 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) { + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + + int px[] = {x}; + 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) { + Display *d = wb_display_get_struct(tag); + if (!d) { + fprintf(stderr, "Error: %s(): invalid device tag.\n", __FUNCTION__); + return; + } + int px[] = {x1, x2}; + 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) { + 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; + } + int px[] = {x, width}; + 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) { + 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; + } + int px[] = {cx, a}; + 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) { + 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) { + 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; + } + int px[] = {x}; + 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) { + 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; + } + int px[] = {x, width}; + 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) { + 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; + } + int px[] = {cx, a}; + 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) { + 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; + 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/webots_ros2_driver/webots/src/controller/c/display_private.h b/webots_ros2_driver/webots/src/controller/c/display_private.h new file mode 100644 index 000000000..4c3261526 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/display_private.h @@ -0,0 +1,22 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef DISPLAY_PRIVATE_H +#define DISPLAY_PRIVATE_H + +int display_get_channel_number(int pixel_format); + +#endif // DISPLAY_PRIVATE_H diff --git a/webots_ros2_driver/webots/src/controller/c/distance_sensor.c b/webots_ros2_driver/webots/src/controller/c/distance_sensor.c new file mode 100644 index 000000000..4577529df --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/distance_sensor.c @@ -0,0 +1,247 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include +#include "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(); + 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(); + 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) { + 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(); + 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(); + 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(); + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/dynamic_library.c b/webots_ros2_driver/webots/src/controller/c/dynamic_library.c new file mode 100644 index 000000000..0b0a04584 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/dynamic_library.c @@ -0,0 +1,86 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/dynamic_library.h b/webots_ros2_driver/webots/src/controller/c/dynamic_library.h new file mode 100644 index 000000000..60b57e8a5 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/dynamic_library.h @@ -0,0 +1,37 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/emitter.c b/webots_ros2_driver/webots/src/controller/c/emitter.c new file mode 100644 index 000000000..1de6521e6 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/emitter.c @@ -0,0 +1,340 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +//*************************************************************************** +// 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(); + WbDevice *d = emitter_get_device(tag); + if (d) { + 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(); + WbDevice *d = emitter_get_device(tag); + if (d) { + 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(); + WbDevice *d = emitter_get_device(tag); + if (d) { + 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/webots_ros2_driver/webots/src/controller/c/file.c b/webots_ros2_driver/webots/src/controller/c/file.c new file mode 100644 index 000000000..c3d3222f9 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/file.c @@ -0,0 +1,27 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "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/webots_ros2_driver/webots/src/controller/c/file.h b/webots_ros2_driver/webots/src/controller/c/file.h new file mode 100644 index 000000000..f6e26f162 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/file.h @@ -0,0 +1,25 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/g_image.c b/webots_ros2_driver/webots/src/controller/c/g_image.c new file mode 100644 index 000000000..8fe20a905 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/g_image.c @@ -0,0 +1,333 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "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]; + 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/webots_ros2_driver/webots/src/controller/c/g_image.h b/webots_ros2_driver/webots/src/controller/c/g_image.h new file mode 100644 index 000000000..969980cba --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/g_image.h @@ -0,0 +1,62 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/g_pipe.c b/webots_ros2_driver/webots/src/controller/c/g_pipe.c new file mode 100644 index 000000000..ff00c1494 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/g_pipe.c @@ -0,0 +1,148 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "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(GPipe *p) { + if (p) + return (size_t)p->handle; + return 0; +} diff --git a/webots_ros2_driver/webots/src/controller/c/g_pipe.h b/webots_ros2_driver/webots/src/controller/c/g_pipe.h new file mode 100644 index 000000000..4d51b0d66 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/g_pipe.h @@ -0,0 +1,55 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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(GPipe *); + +#ifdef __cplusplus +} +#endif + +#endif // G_PIPE_H diff --git a/webots_ros2_driver/webots/src/controller/c/gps.c b/webots_ros2_driver/webots/src/controller/c/gps.c new file mode 100644 index 000000000..5d8bd99e0 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/gps.c @@ -0,0 +1,231 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include "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) { + 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(); + 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(); + 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(); + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/gyro.c b/webots_ros2_driver/webots/src/controller/c/gyro.c new file mode 100644 index 000000000..71a268b35 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/gyro.c @@ -0,0 +1,191 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include "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(); + 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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/html_robot_window.c b/webots_ros2_driver/webots/src/controller/c/html_robot_window.c new file mode 100644 index 000000000..5dc4d481b --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/html_robot_window.c @@ -0,0 +1,112 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// 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/webots_ros2_driver/webots/src/controller/c/html_robot_window_private.h b/webots_ros2_driver/webots/src/controller/c/html_robot_window_private.h new file mode 100644 index 000000000..e6bbb4f9f --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/html_robot_window_private.h @@ -0,0 +1,25 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/image.c b/webots_ros2_driver/webots/src/controller/c/image.c new file mode 100644 index 000000000..7b427bb39 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/image.c @@ -0,0 +1,74 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include "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/webots_ros2_driver/webots/src/controller/c/image_private.h b/webots_ros2_driver/webots/src/controller/c/image_private.h new file mode 100644 index 000000000..d443ca6e9 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/image_private.h @@ -0,0 +1,39 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/inertial_unit.c b/webots_ros2_driver/webots/src/controller/c/inertial_unit.c new file mode 100644 index 000000000..4d6e7877f --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/inertial_unit.c @@ -0,0 +1,195 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "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(); + 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) { + 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(); + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/joystick.c b/webots_ros2_driver/webots/src/controller/c/joystick.c new file mode 100644 index 000000000..8ce285395 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/joystick.c @@ -0,0 +1,352 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include "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/webots_ros2_driver/webots/src/controller/c/joystick_private.h b/webots_ros2_driver/webots/src/controller/c/joystick_private.h new file mode 100644 index 000000000..2816b4c27 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/joystick_private.h @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/keyboard.c b/webots_ros2_driver/webots/src/controller/c/keyboard.c new file mode 100644 index 000000000..cf7b45aa0 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/keyboard.c @@ -0,0 +1,128 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include "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/webots_ros2_driver/webots/src/controller/c/keyboard_private.h b/webots_ros2_driver/webots/src/controller/c/keyboard_private.h new file mode 100644 index 000000000..dce45bcd7 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/keyboard_private.h @@ -0,0 +1,28 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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/webots_ros2_driver/webots/src/controller/c/led.c b/webots_ros2_driver/webots/src/controller/c/led.c new file mode 100644 index 000000000..12e71d185 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/led.c @@ -0,0 +1,102 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +// *************************************************** +// 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(); + 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/webots_ros2_driver/webots/src/controller/c/lidar.c b/webots_ros2_driver/webots/src/controller/c/lidar.c new file mode 100644 index 000000000..fe5911e21 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/lidar.c @@ -0,0 +1,516 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "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(WbDevice *d) { + 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) { + 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) { + 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(); + 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(); + 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(); + 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(); + 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(); + 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(); + 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) { + 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(); + 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) { + 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(); + 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) { + 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) { + 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) { + 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) { + 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) { + 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/webots_ros2_driver/webots/src/controller/c/light_sensor.c b/webots_ros2_driver/webots/src/controller/c/light_sensor.c new file mode 100644 index 000000000..aa6c7e5fb --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/light_sensor.c @@ -0,0 +1,186 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "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(); + 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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/messages.h b/webots_ros2_driver/webots/src/controller/c/messages.h new file mode 100644 index 000000000..c4f2fa9d5 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/messages.h @@ -0,0 +1,304 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#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 + +// 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 + +// 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/webots_ros2_driver/webots/src/controller/c/microphone.c b/webots_ros2_driver/webots/src/controller/c/microphone.c new file mode 100644 index 000000000..f3e0e3d93 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/microphone.c @@ -0,0 +1,181 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include +#include "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(); + 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) { + 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(); + 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(); + 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/webots_ros2_driver/webots/src/controller/c/motion.c b/webots_ros2_driver/webots/src/controller/c/motion.c new file mode 100644 index 000000000..195140af4 --- /dev/null +++ b/webots_ros2_driver/webots/src/controller/c/motion.c @@ -0,0 +1,560 @@ +/* + * Copyright 1996-2022 Cyberbotics Ltd. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +//----------------------------------------------------------- +// 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"; + +// 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 + 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; + } + + 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