Added support for CoppeliaSim v4.5

Breaking change in the model (initializer). Removed all simXXX types aliases but simFloat that is aliased to double or float depending on the version (float <=4.4, double >4.5). Replaced - simGetVisionSensorCharImage with simGetVisionSensorImg - simSetInt32Parameter with simSetInt32Param (and similar) - simSetIntegerSignal with simSetInt32Signal - simGetObjectHandle with simGetObject Added support for `include` in CMakeLists.txt in addition to `libPlugin`
jeguzzi · May 13, 2023 · c243bdb · c243bdb
1 parent 676efc1
commit c243bdb
Show file tree

Hide file tree

Showing 12 changed files with 208 additions and 91 deletions.
diff --git a/.gitignore b/.gitignore
@@ -41,4 +41,5 @@ include/spdlog
 coppeliaSim_plugin_debug
 linux_build
 xcode
+build*
 __pycache__
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -0,0 +1,34 @@
+# Changelog
+
+## [Unreleased]
+
+### Added
+
+- Support for CoppeliaSim v4.5. The plugin supports now versions from 4.0 to 4.5.
+
+### Changed
+
+- The model was changed to support CoppeliaSim 4.5. In particular the initializer
+  `simRobomaster.create` and `simRobomaster.create_{ep|s1}` now take the model handle as first argument,
+  while previously they took the model index. When using older scenes with Robomasters, 
+  users need to update they robots to use the new initializer, 
+  even if they are using a previous version of CoppeliaSim.
+- The two scenes `playground_tof_{ep|s1}.ttt` were updated with the new robot models.
+
+### Known issues
+
+- The initial release Coppelia v4.5 has [a bug](https://forum.coppeliarobotics.com/viewtopic.php?p=38863) 
+  in the lua script of `simIK`. If you experience errors
+  using the Robomaster gripper, add the following lines at the begin 
+  of `simIK.debugGroupIfNeeded` in `simIK.lua`
+  ```lua
+  function simIK.debugGroupIfNeeded(ikEnv,ikGroup,debugFlags)
+    if not _S.ikEnvs then
+        _S.ikEnvs={}
+    end
+    ...
+  ```	
+
+## [0.1] - 2023-04-05
+
+Initial release
diff --git a/README.md b/README.md
@@ -40,14 +40,18 @@ On Linux, we require a C++-20 compiler.
 
 ### CoppeliaSim
 
-To compile and then use the CoppeliaSim plugin you need ... [CoppeliaSim](https://www.coppeliarobotics.com).
-Download release 4.4, as we do not yet support the lastest release. Export the location where you place it.
+To compile and then use the CoppeliaSim plugin you need ... [CoppeliaSim](https://www.coppeliarobotics.com). We support CoppeliaSim from v4 on.
+Download the latest release. Export the location where you place it.
 
 ```bash
 export COPPELIASIM_ROOT_DIR=<path to the folder containing the programming subfolder>
 ```
 which on Linux is the root folder that you download, while on MacOs is `/Applications/coppeliaSim.app/Contents/Resources`, if you install the app to the default location.
 
+You also need to install Python>=3.8 and two common dependencies for CoppeliaSim plugins:
+- [xsltproc](http://xmlsoft.org/xslt/xsltproc.html). Linux: `sudo apt install xsltproc`. MacOs comes with `xsltproc` installed.
+- [xmlschema](https://github.com/sissaschool/xmlschema) (optional): `python3 -m pip install xmlschema`
+
 ## Build
 
 #### Linux and MacOs
@@ -95,7 +99,7 @@ Then run one of the Python scripts in `examples`
 $ cd <this repo>/examples
 $ python discovery.py
 ```
-which discovers any Robomaster avaible on the network.
+which discovers any Robomaster available on the network.
 
 Other scripts showcase various parts of the remote API. For instance
 ```
@@ -209,3 +213,19 @@ and
       handle = simRobomaster.create_s1(index, "127.0.1.1/8", "RM1")
   end
 ```
+
+### Trubleshooting
+
+#### simIK
+
+The initial release of Coppelia v4.5 has [a bug](https://forum.coppeliarobotics.com/viewtopic.php?p=38863) 
+in the lua script of `simIK`. If you experience errors
+using the Robomaster gripper, add the following lines at the begin 
+of `simIK.debugGroupIfNeeded` in `simIK.lua`
+```lua
+function simIK.debugGroupIfNeeded(ikEnv,ikGroup,debugFlags)
+  if not _S.ikEnvs then
+      _S.ikEnvs={}
+  end
+  ...
+``` 
diff --git a/coppeliaSim_plugin/CMakeLists.txt b/coppeliaSim_plugin/CMakeLists.txt
@@ -1,21 +1,30 @@
 set(CMAKE_INCLUDE_CURRENT_DIR ON)
 set(CMAKE_MACOSX_RPATH 1)
 
+ if(NOT COPPELIASIM_INCLUDE_DIR)
+    if(DEFINED ENV{COPPELIASIM_ROOT_DIR})
+        set(COPPELIASIM_INCLUDE_DIR $ENV{COPPELIASIM_ROOT_DIR}/programming/include)
+    else()
+        set(COPPELIASIM_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../include)
+    endif()
+endif()
 if(NOT LIBPLUGIN_DIR)
     if(DEFINED ENV{COPPELIASIM_ROOT_DIR})
         set(LIBPLUGIN_DIR $ENV{COPPELIASIM_ROOT_DIR}/programming/libPlugin)
-        set(MODEL_FOLDER $ENV{COPPELIASIM_ROOT_DIR}/models/robots/mobile)
-        set(MODEL_SENSOR_FOLDER $ENV{COPPELIASIM_ROOT_DIR}/models/components/sensors)
-        install(FILES ../models/RoboMasterEP.ttm ../models/RoboMasterS1.ttm  DESTINATION ${MODEL_FOLDER})
-        install(FILES ../models/RoboMasterDistanceSensor.ttm DESTINATION ${MODEL_SENSOR_FOLDER})
     else()
         set(LIBPLUGIN_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../libPlugin)
     endif()
 endif()
 
+
+set(MODEL_FOLDER $ENV{COPPELIASIM_ROOT_DIR}/models/robots/mobile)
+set(MODEL_SENSOR_FOLDER $ENV{COPPELIASIM_ROOT_DIR}/models/components/sensors)
+
 list(APPEND CMAKE_MODULE_PATH
     ${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules
+    ${COPPELIASIM_INCLUDE_DIR}/cmake
     ${LIBPLUGIN_DIR}/cmake)
+
 find_package(CoppeliaSim 4 REQUIRED)
 
 include_directories(${CMAKE_CURRENT_BINARY_DIR}/generated)
@@ -26,3 +35,6 @@ coppeliasim_generate_stubs(${CMAKE_CURRENT_BINARY_DIR}/generated XML_FILE ${CMAK
 configure_file(${CMAKE_CURRENT_SOURCE_DIR}/config.h.in ${CMAKE_CURRENT_BINARY_DIR}/config.h ESCAPE_QUOTES)
 
 coppeliasim_add_plugin(simExtRobomaster SOURCES plugin.cpp coppeliasim_robot.cpp)
+
+install(FILES ../models/RoboMasterEP.ttm ../models/RoboMasterS1.ttm  DESTINATION ${MODEL_FOLDER})
+install(FILES ../models/RoboMasterDistanceSensor.ttm DESTINATION ${MODEL_SENSOR_FOLDER})
diff --git a/coppeliaSim_plugin/callbacks.xml b/coppeliaSim_plugin/callbacks.xml
@@ -7,8 +7,8 @@
     <command name="create">
         <description>Instantiate a RoboMaster controller</description>
         <params>
-          <param name="index" type="int">
-            <description>The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix</description>
+          <param name="handle" type="int">
+            <description>The handle of the CoppeliaSim robot.</description>
           </param>
           <param name="remote_api_network" type="string" default='""'>
               <description>The address that the remote API should use `"&lt;ip&gt;/&lt;subnet size in bits&gt;"`. E.g., use `"127.0.0.1/24"` for local network, `"/0"` to bind to any network interface. Leave empty to disable the remote API.</description>
@@ -50,8 +50,8 @@
           <category name="ep"/>
         </categories>
         <params>
-          <param name="index" type="int">
-            <description>The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix</description>
+          <param name="handle" type="int">
+            <description>The handle of the CoppeliaSim robot.</description>
           </param>
           <param name="remote_api_network" type="string" default='""'>
               <description>The address that the remote API should use `"&lt;ip&gt;/&lt;subnet size in bits&gt;"`. E.g., use `"127.0.0.1/24"` for local network, `"/0"` to bind to any network interface. Leave empty to disable the remote API.</description>
@@ -75,8 +75,8 @@
           <category name="s1"/>
         </categories>
         <params>
-          <param name="index" type="int">
-            <description>The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix</description>
+          <param name="handle" type="int">
+            <description>The handle of the CoppeliaSim robot.</description>
           </param>
           <param name="remote_api_network" type="string" default='""'>
               <description>The address that the remote API should use `"&lt;ip&gt;/&lt;subnet size in bits&gt;"`. E.g., use `"127.0.0.1/24"` for local network, `"/0"` to bind to any network interface. Leave empty to disable the remote API.</description>

diff --git a/coppeliaSim_plugin/coppeliasim_robot.cpp b/coppeliaSim_plugin/coppeliasim_robot.cpp
@@ -5,11 +5,11 @@
 #include "coppeliasim_robot.hpp"
 
 // bool read_vector_from_signal(const std::string name, Vector3 *out) {
-//   simInt buffer_lenght;
-//   simChar *buffer = simGetStringSignal(name.data(), &buffer_lenght);
+//   int buffer_lenght;
+//   char *buffer = simGetStringSignal(name.data(), &buffer_lenght);
 //   if (buffer && buffer_lenght == sizeof(Vector3)) {
-//     simInt stack = simCreateStack();
-//     simInt r = simUnpackTable(stack, buffer, buffer_lenght);
+//     int stack = simCreateStack();
+//     int r = simUnpackTable(stack, buffer, buffer_lenght);
 //     if (r < 0) {
 //       return false;
 //     }
@@ -19,8 +19,8 @@
 // }
 
 bool read_vector_from_signal(const std::string name, Vector3 *out) {
-  simInt buffer_lenght;
-  simChar *buffer = simGetStringSignal(name.data(), &buffer_lenght);
+  int buffer_lenght;
+  char *buffer = simGetStringSignal(name.data(), &buffer_lenght);
   if (buffer && buffer_lenght == sizeof(Vector3)) {
     memcpy(&(out->x), buffer, buffer_lenght);
     return true;
@@ -40,7 +40,7 @@ void CoppeliaSimRobot::forward_chassis_led(size_t index, const Color &rgb) {
     spdlog::warn("Unknown chassis led {}", index);
     return;
   }
-  simFloat color[3] = {rgb.r, rgb.g, rgb.b};
+  float color[3] = {rgb.r, rgb.g, rgb.b};
   spdlog::debug("Set led {} color to {} {} {}", index, color[0], color[1], color[2]);
   simSetShapeColor(chassis_led_handles[index], nullptr, sim_colorcomponent_emission, color);
 }
@@ -50,7 +50,7 @@ void CoppeliaSimRobot::forward_gimbal_led(size_t index, size_t part, const Color
     spdlog::warn("Unknown gimbal led {}[{}]", index, part);
     return;
   }
-  simFloat color[3] = {rgb.r, rgb.g, rgb.b};
+  float color[3] = {rgb.r, rgb.g, rgb.b};
   spdlog::debug("Set gimbal led {}-{} color to {} {} {} [{}]", index, part, color[0], color[1],
                 color[2], gimbal_led_handles[index][part]);
   simSetShapeColor(gimbal_led_handles[index][part], nullptr, sim_colorcomponent_emission, color);
@@ -59,7 +59,7 @@ void CoppeliaSimRobot::forward_gimbal_led(size_t index, size_t part, const Color
 void CoppeliaSimRobot::forward_blaster_led(float value) const {
   if (blaster_light_handle <= 0)
     return;
-  const simFloat rgb[3] = {0.0f, std::clamp(value, 0.0f, 1.0f), 0.0f};
+  const float rgb[3] = {0.0f, std::clamp(value, 0.0f, 1.0f), 0.0f};
   if (value)
     simSetLightParameters(blaster_light_handle, 1, nullptr, rgb, nullptr);
   else
@@ -69,7 +69,11 @@ void CoppeliaSimRobot::forward_blaster_led(float value) const {
 WheelSpeeds CoppeliaSimRobot::read_wheel_speeds() const {
   WheelSpeeds value;
   for (size_t i = 0; i < value.size; i++) {
-    simGetObjectFloatParameter(wheel_joint_handles[i], sim_jointfloatparam_velocity, &value[i]);
+    // I need to read a simFloat because from version 4.5, coppeliaSim uses double instead of floats
+    // in most apis.
+    simFloat v;
+    simGetObjectFloatParam(wheel_joint_handles[i], sim_jointfloatparam_velocity, &v);
+    value[i] = v;
   }
   return value;
 }
@@ -125,9 +129,14 @@ std::vector<uint8_t> CoppeliaSimRobot::read_camera_image() const {
   if (!camera_handle)
     return {};
   simHandleVisionSensor(camera_handle, nullptr, nullptr);
-  simInt width = 0;
-  simInt height = 0;
-  simUChar *buffer = simGetVisionSensorCharImage(camera_handle, &width, &height);
+  int resolution[2]{};
+#if SIM_PROGRAM_VERSION_NB >= 40400
+  unsigned char *buffer = simGetVisionSensorImg(camera_handle, 0, 0.0, nullptr, nullptr, resolution);
+#else
+  unsigned char *buffer = simGetVisionSensorCharImage(camera_handle, &width, &height);
+#endif
+  const int width = resolution[0];
+  const int height = resolution[1];
   if (width != camera.width || height != camera.height) {
     spdlog::warn("Skip frame because of uncorrect size ({}, {}) vs desired size ({}, {})", width,
                  height, camera.width, camera.height);
@@ -137,24 +146,28 @@ std::vector<uint8_t> CoppeliaSimRobot::read_camera_image() const {
   // std::vector image(buffer, buffer + size);
   std::vector<uint8_t> image;
   image.reserve(size);
-  simInt resolution[2] = {width, height};
+  // int resolution[2] = {width, height};
   simTransformImage(buffer, resolution, 4, nullptr, nullptr, nullptr);
 
   std::copy(buffer, buffer + size, std::back_inserter(image));
   // image = std::vector(image);
-  simReleaseBuffer((const simChar *)buffer);
+  simReleaseBuffer((const char *)buffer);
   spdlog::debug("Got a {} x {} from CoppeliaSim", width, height);
   return image;
 }
 
 bool CoppeliaSimRobot::forward_camera_resolution(unsigned width, unsigned height) {
   if (!camera_handle)
     return false;
-  simInt image_size[2]{};
+  int image_size[2]{};
+#if SIM_PROGRAM_VERSION_NB >= 40500
+  simGetVisionSensorRes(camera_handle, image_size);
+#else
   simGetVisionSensorResolution(camera_handle, image_size);
+#endif
   if (width != image_size[0] || height != image_size[1]) {
-    simSetObjectInt32Parameter(camera_handle, sim_visionintparam_resolution_x, width);
-    simSetObjectInt32Parameter(camera_handle, sim_visionintparam_resolution_y, height);
+    simSetObjectInt32Param(camera_handle, sim_visionintparam_resolution_x, width);
+    simSetObjectInt32Param(camera_handle, sim_visionintparam_resolution_y, height);
     spdlog::warn("Changing camera resolution from ({}, {}) to ({}, {})", image_size[0],
                  image_size[1], width, height);
     return true;
@@ -171,16 +184,16 @@ void CoppeliaSimRobot::forward_target_servo_angle(size_t index, float angle) {
 void CoppeliaSimRobot::forward_servo_mode(size_t index, Servo::Mode mode) {
   if (servo_handles.at(index) > 0) {
     if (mode == Servo::ANGLE) {
-      simSetObjectInt32Parameter(servo_handles.at(index), sim_jointintparam_ctrl_enabled, 1);
+      simSetObjectInt32Param(servo_handles.at(index), sim_jointintparam_ctrl_enabled, 1);
     } else {
-      simSetObjectInt32Parameter(servo_handles.at(index), sim_jointintparam_ctrl_enabled, 0);
+      simSetObjectInt32Param(servo_handles.at(index), sim_jointintparam_ctrl_enabled, 0);
     }
   }
 }
 
 void CoppeliaSimRobot::forward_servo_enabled(size_t index, bool value) {
   if (servo_handles.at(index) > 0) {
-    simSetObjectInt32Parameter(servo_handles.at(index), sim_jointintparam_motor_enabled, value);
+    simSetObjectInt32Param(servo_handles.at(index), sim_jointintparam_motor_enabled, value);
   }
 }
 
@@ -192,17 +205,17 @@ void CoppeliaSimRobot::forward_target_servo_speed(size_t index, float speed) {
 }
 
 float CoppeliaSimRobot::read_servo_angle(size_t index) const {
-  float angle = 0;
+  simFloat angle = 0;
   if (servo_handles.at(index) > 0) {
     simGetJointPosition(servo_handles.at(index), &angle);
   }
   return angle;
 }
 
 float CoppeliaSimRobot::read_servo_speed(size_t index) const {
-  float speed = 0;
+  simFloat speed = 0;
   if (servo_handles.at(index) > 0) {
-    simGetObjectFloatParameter(servo_handles.at(index), sim_jointfloatparam_velocity, &speed);
+    simGetObjectFloatParam(servo_handles.at(index), sim_jointfloatparam_velocity, &speed);
   }
   return speed;
 }
@@ -211,16 +224,24 @@ void CoppeliaSimRobot::forward_target_gripper(Gripper::Status state, float power
   if (gripper_target_signal.empty()) {
     spdlog::warn("Gripper not available");
   }
+#if SIM_PROGRAM_VERSION_NB >= 40300
+  simSetInt32Signal(gripper_target_signal.data(), static_cast<int>(state));
+#else
   simSetIntegerSignal(gripper_target_signal.data(), static_cast<int>(state));
+#endif
 }
 
 Gripper::Status CoppeliaSimRobot::read_gripper_state() const {
   if (gripper_state_signal.empty()) {
     spdlog::warn("Gripper not available");
     return Gripper::Status::pause;
   }
-  simInt value;
+  int value;
+#if SIM_PROGRAM_VERSION_NB >= 40300
+  simGetInt32Signal(gripper_target_signal.data(), &value);
+#else  
   simGetIntegerSignal(gripper_state_signal.data(), &value);
+#endif
   return Gripper::Status(value);
 }
 
@@ -230,9 +251,9 @@ hit_event_t CoppeliaSimRobot::read_hit_events() const { return {}; }
 
 float CoppeliaSimRobot::read_tof(size_t index) const {
   if (!tof_handles.count(index)) return 0.0f;
-  simInt h = tof_handles.at(index);
+  int h = tof_handles.at(index);
   simFloat p[4];
-  simInt r = simCheckProximitySensor(h, sim_handle_all, p);
+  int r = simCheckProximitySensor(h, sim_handle_all, p);
   if (r == 1) {
     return p[3];
   } else if (r == 0) {
@@ -243,14 +264,14 @@ float CoppeliaSimRobot::read_tof(size_t index) const {
 
 ir_event_t CoppeliaSimRobot::read_ir_events() const { return {}; }
 
-void CoppeliaSimRobot::enable_tof(size_t index, simInt sensor_handle) {
+void CoppeliaSimRobot::enable_tof(size_t index, int sensor_handle) {
   if (index < MAX_NUMBER_OF_TOF_SENSORS && Robot::enable_tof(index)) {
     tof_handles[index] = sensor_handle;
   }
 }
 
 void CoppeliaSimRobot::forward_engage_wheel_motors(bool value) {
-  simInt v = value ? 1 : 0;
+  int v = value ? 1 : 0;
 
   for (size_t i = 0; i < wheel_joint_handles.size; i++) {
     spdlog::info("[Chassis] Set joint {} motor enabled to {}", wheel_joint_handles[i], v);