diff --git a/.gitignore b/.gitignore index 1fe19ce..1721db6 100644 --- 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 new file mode 100644 index 0000000..941bb1b --- /dev/null +++ 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 index 8aa0dd7..c13f86e 100644 --- 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= ``` 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 /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 index 190ea3b..c1ab878 100644 --- 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 index d3091b1..8a001f8 100644 --- a/coppeliaSim_plugin/callbacks.xml +++ b/coppeliaSim_plugin/callbacks.xml @@ -7,8 +7,8 @@ Instantiate a RoboMaster controller - - The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix + + The handle of the CoppeliaSim robot. The address that the remote API should use `"<ip>/<subnet size in bits>"`. 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. @@ -50,8 +50,8 @@ - - The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix + + The handle of the CoppeliaSim robot. The address that the remote API should use `"<ip>/<subnet size in bits>"`. 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. @@ -75,8 +75,8 @@ - - The suffix of the CoppeliaSim robot. Use `-1` for an empty suffix + + The handle of the CoppeliaSim robot. The address that the remote API should use `"<ip>/<subnet size in bits>"`. 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. diff --git a/coppeliaSim_plugin/coppeliasim_robot.cpp b/coppeliaSim_plugin/coppeliasim_robot.cpp index ae6cc2e..71009b3 100644 --- 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 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,12 +146,12 @@ std::vector CoppeliaSimRobot::read_camera_image() const { // std::vector image(buffer, buffer + size); std::vector 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; } @@ -150,11 +159,15 @@ std::vector CoppeliaSimRobot::read_camera_image() const { 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,7 +205,7 @@ 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); } @@ -200,9 +213,9 @@ float CoppeliaSimRobot::read_servo_angle(size_t index) const { } 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,7 +224,11 @@ 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(state)); +#else simSetIntegerSignal(gripper_target_signal.data(), static_cast(state)); +#endif } Gripper::Status CoppeliaSimRobot::read_gripper_state() const { @@ -219,8 +236,12 @@ Gripper::Status CoppeliaSimRobot::read_gripper_state() const { 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); diff --git a/coppeliaSim_plugin/coppeliasim_robot.hpp b/coppeliaSim_plugin/coppeliasim_robot.hpp index d4c609b..fddf4d6 100644 --- a/coppeliaSim_plugin/coppeliasim_robot.hpp +++ b/coppeliaSim_plugin/coppeliasim_robot.hpp @@ -9,15 +9,21 @@ #include +#if SIM_PROGRAM_VERSION_NB >= 40500 +typedef double simFloat; +#else +typedef float simFloat; +#endif + class CoppeliaSimRobot : public Robot { public: - CoppeliaSimRobot(WheelValues _wheel_joint_handles, ChassisLEDValues _led_handles, - bool _enable_arm, simInt _camera_handle, ServoValues _servo_motor, - GimbalValues _gimbal_motor, - GimbalLEDValues> _gimbal_led_handles, - simInt _blaster_light_handle, bool _enable_gripper, + CoppeliaSimRobot(WheelValues _wheel_joint_handles, ChassisLEDValues _led_handles, + bool _enable_arm, int _camera_handle, ServoValues _servo_motor, + GimbalValues _gimbal_motor, + GimbalLEDValues> _gimbal_led_handles, + int _blaster_light_handle, bool _enable_gripper, std::string _gripper_state_signal, std::string _gripper_target_signal, - simInt _imu_handle, std::string _accelerometer_signal, std::string _gyro_signal) + int _imu_handle, std::string _accelerometer_signal, std::string _gyro_signal) : Robot(_enable_arm, _enable_gripper, {_servo_motor[0] > 0, _servo_motor[1] > 0, _servo_motor[2] > 0}, (_gimbal_motor.yaw > 0 && _gimbal_motor.pitch > 0), _camera_handle > 0, false) @@ -61,7 +67,7 @@ class CoppeliaSimRobot : public Robot { void forward_target_gimbal_angle(const GimbalValues &angle); void forward_blaster_led(float value) const; void forward_engage_wheel_motors(bool value); - void enable_tof(size_t index, simInt sensor_handle); + void enable_tof(size_t index, int sensor_handle); float read_tof(size_t index) const; // void has_read_accelerometer(float x, float y, float z); @@ -69,16 +75,16 @@ class CoppeliaSimRobot : public Robot { // void update_orientation(float alpha, float beta, float gamma); private: - WheelValues wheel_joint_handles; - ChassisLEDValues chassis_led_handles; - GimbalLEDValues> gimbal_led_handles; - simInt blaster_light_handle; + WheelValues wheel_joint_handles; + ChassisLEDValues chassis_led_handles; + GimbalLEDValues> gimbal_led_handles; + int blaster_light_handle; const std::map servo_handles; std::map tof_handles; - simInt camera_handle; + int camera_handle; std::string gripper_state_signal; std::string gripper_target_signal; - simInt imu_handle; + int imu_handle; std::string accelerometer_signal; std::string gyro_signal; }; diff --git a/coppeliaSim_plugin/plugin.cpp b/coppeliaSim_plugin/plugin.cpp index 5004431..9621341 100644 --- a/coppeliaSim_plugin/plugin.cpp +++ b/coppeliaSim_plugin/plugin.cpp @@ -35,10 +35,11 @@ #include "config.h" #include "plugin.h" -#include "simPlusPlus/Handle.h" #include "simPlusPlus/Plugin.h" #include "stubs.h" +#include + CS_Vector3 to_cs(const Vector3 &value) { return CS_Vector3(value.x, value.y, value.z); } CS_Attitude to_cs(const Attitude &value) { return CS_Attitude(value.yaw, value.pitch, value.roll); } @@ -80,17 +81,40 @@ static const GimbalValues gimbal_servo_names = {.yaw = "gimbal_yaw_ static const char servo_name[] = "servo_motor_"; static const char camera_name[] = "Vision_sensor"; static const char blaster_light_name[] = "blaster_light"; - +static const char gripper_name[] = "gripper_link_respondable"; +static const char gyro_name[] = "GyroSensor"; static const char imu_name[] = "GyroSensor_reference"; -static const char accelerometer_signal_name[] = "accelerometer"; -static const char gyro_signal_name[] = "gyro"; +static const char accelerometer_name[] = "Accelerometer"; + + +#if SIM_PROGRAM_VERSION_NB < 40300 -static int get_handle(std::string name, std::string suffix) { - std::string complete_name = name + suffix + "@silentError"; - spdlog::info("Getting handle of {}", complete_name); - return simGetObjectHandle(complete_name.data()); +static int get_index(int handle) { + const char * name = simGetObjectName(handle); + return simGetNameSuffix(name); +} + +static int get_handle(std::string name, int handle) { + std::string suffix = "#"; + const int index = get_index(handle); + if (index >= 0) { + suffix += std::to_string(index); + } + name = name + suffix + "@silentError"; + spdlog::info("Getting handle of {}", name); + return simGetObjectHandle(name.data()); } +#else + +static int get_handle(std::string name, int handle) { + name = "./" + name; + spdlog::info("Getting handle of {} in {}", name, handle); + return simGetObject(name.data(), -1, handle, 1); +} + +#endif + static std::string get_ip(std::string network, unsigned *prefix_len) { size_t pos = network.find_first_of('/'); std::string ip = network.substr(0, pos); @@ -102,29 +126,25 @@ static std::string get_ip(std::string network, unsigned *prefix_len) { return ip; } -static int add_robot(int coppelia_index, std::string serial_number, +static int add_robot(int cs_handle, std::string serial_number, std::string remote_api_network = "", bool enable_camera = true, bool camera_use_udp = false, int camera_bitrate = 1000000, bool enable_arm = true, bool enable_gripper = true, bool enable_gimbal = true) { int handle = next_robot_handle; - std::string suffix = "#"; - if (coppelia_index >= 0) { - suffix += std::to_string(coppelia_index); - } int camera_handle = -1; if (enable_camera) { - camera_handle = get_handle(camera_name, suffix); + camera_handle = get_handle(camera_name, cs_handle); if (camera_handle == -1) { spdlog::error("Could not locate camera"); enable_camera = false; } } - WheelValues wheel_handles; + WheelValues wheel_handles; for (size_t i = 0; i < wheel_handles.size; i++) { - simInt h = get_handle(wheel_names[i], suffix); + int h = get_handle(wheel_names[i], cs_handle); if (h == -1) { spdlog::error("Could not locate wheel #{}", i); return -1; @@ -132,9 +152,9 @@ static int add_robot(int coppelia_index, std::string serial_number, wheel_handles[i] = h; } - ChassisLEDValues chassis_led_handles; + ChassisLEDValues chassis_led_handles; for (size_t i = 0; i < chassis_led_handles.size; i++) { - simInt h = get_handle(chassis_led_names[i], suffix); + int h = get_handle(chassis_led_names[i], cs_handle); if (h == -1) { spdlog::error("Could not locate led #{}", i); return -1; @@ -142,33 +162,33 @@ static int add_robot(int coppelia_index, std::string serial_number, chassis_led_handles[i] = h; } - ServoValues servo_motors; + ServoValues servo_motors; for (size_t i = 0; i < servo_motors.size(); i++) { - servo_motors[i] = get_handle(servo_name + std::to_string(i), suffix); + servo_motors[i] = get_handle(servo_name + std::to_string(i), cs_handle); if (servo_motors[i] == -1 && i < 2 && enable_arm) { spdlog::error("Could not locate arm servo motor #{}", i); enable_arm = false; } } - simInt blaster_light_handle = -1; - GimbalValues gimbal_motors{-1, -1}; + int blaster_light_handle = -1; + GimbalValues gimbal_motors{-1, -1}; if (enable_gimbal) { for (size_t i = 0; i < gimbal_motors.size; i++) { - gimbal_motors[i] = get_handle(gimbal_servo_names[i], suffix); + gimbal_motors[i] = get_handle(gimbal_servo_names[i], cs_handle); if (gimbal_motors[i] == -1) { spdlog::error("Could not locate gimbal motor #{}", i); enable_gimbal = false; break; } } - blaster_light_handle = get_handle(blaster_light_name, suffix); + blaster_light_handle = get_handle(blaster_light_name, cs_handle); } - GimbalLEDValues> gimbal_led_handles; + GimbalLEDValues> gimbal_led_handles; if (enable_gimbal) { for (size_t i = 0; i < gimbal_led_handles.size; i++) { for (size_t j = 0; j < 8; j++) { - simInt h = get_handle(gimbal_led_names[i] + std::to_string(j), suffix); + int h = get_handle(gimbal_led_names[i] + std::to_string(j), cs_handle); if (h == -1) { spdlog::error("Could not locate gimbal led #{}_{}", i, j); return -1; @@ -177,15 +197,18 @@ static int add_robot(int coppelia_index, std::string serial_number, } } } - std::string gripper_state = "gripper" + suffix; - std::string gripper_target = "target_gripper" + suffix; + int gripper_handle = get_handle(gripper_name, cs_handle); + std::string gripper_state = "gripper#" + std::to_string(gripper_handle); + std::string gripper_target = "target_gripper#" + std::to_string(gripper_handle); spdlog::info("Gripper signals {} {}", gripper_state, gripper_target); - int imu_handle = get_handle(imu_name, suffix); - std::string accelerometer_signal = accelerometer_signal_name + suffix; - std::string gyro_signal = gyro_signal_name + suffix; - + int imu_handle = get_handle(imu_name, cs_handle); + int gyro_handle = get_handle(gyro_name, cs_handle); + std::string gyro_signal = "gyro#" + std::to_string(gyro_handle); + int accelerometer_handle = get_handle(accelerometer_name, cs_handle); + std::string accelerometer_signal = "accelerometer#" + std::to_string(accelerometer_handle); + _robots.emplace(handle, std::make_unique( wheel_handles, chassis_led_handles, enable_arm, camera_handle, servo_motors, gimbal_motors, gimbal_led_handles, blaster_light_handle, @@ -246,18 +269,18 @@ class Plugin : public sim::Plugin { } void create(create_in *in, create_out *out) { - out->handle = add_robot(in->index, in->serial_number, in->remote_api_network, in->enable_camera, + out->handle = add_robot(in->handle, in->serial_number, in->remote_api_network, in->enable_camera, in->camera_use_udp, in->camera_bitrate, in->enable_arm, in->enable_gripper, in->enable_gimbal); } void create_ep(create_ep_in *in, create_ep_out *out) { - out->handle = add_robot(in->index, in->serial_number, in->remote_api_network, true, false, + out->handle = add_robot(in->handle, in->serial_number, in->remote_api_network, true, false, 1000000, true, true, false); } void create_s1(create_s1_in *in, create_s1_out *out) { - out->handle = add_robot(in->index, in->serial_number, in->remote_api_network, true, false, + out->handle = add_robot(in->handle, in->serial_number, in->remote_api_network, true, false, 1000000, false, false, true); } @@ -606,7 +629,7 @@ class Plugin : public sim::Plugin { // void onModuleHandle(char *customData) { // std::string module = customData ? std::string(customData) : "ALL"; - simFloat time_step = simGetSimulationTimeStep(); + auto time_step = simGetSimulationTimeStep(); // simFloat time_ = simGetSimulationTime(); // spdlog::debug("onModuleHandle {} {} ({})", module, time_step, time_); for (auto const &[key, val] : _robots) { diff --git a/models/RoboMasterEP.ttm b/models/RoboMasterEP.ttm index 288261e..3b0f936 100644 Binary files a/models/RoboMasterEP.ttm and b/models/RoboMasterEP.ttm differ diff --git a/models/RoboMasterS1.ttm b/models/RoboMasterS1.ttm index bdfd09b..eb68e9a 100644 Binary files a/models/RoboMasterS1.ttm and b/models/RoboMasterS1.ttm differ diff --git a/scenes/playground_tof_ep.ttt b/scenes/playground_tof_ep.ttt index 202a3db..d69a5df 100644 Binary files a/scenes/playground_tof_ep.ttt and b/scenes/playground_tof_ep.ttt differ diff --git a/scenes/playground_tof_s1.ttt b/scenes/playground_tof_s1.ttt index fb61a34..bd2a1a1 100644 Binary files a/scenes/playground_tof_s1.ttt and b/scenes/playground_tof_s1.ttt differ