diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitignore b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitignore new file mode 100644 index 0000000..046aa17 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitignore @@ -0,0 +1,53 @@ +# These are some examples of commonly ignored file patterns. +# You should customize this list as applicable to your project. +# Learn more about .gitignore: +# https://www.atlassian.com/git/tutorials/saving-changes/gitignore + +# Build folder +build/ + +# Node artifact files +node_modules/ +dist/ + +# Compiled Java class files +*.class + +# Compiled Python bytecode +*.py[cod] + +# Log files +*.log + +# Package files +*.jar + +# Maven +target/ +dist/ + +# JetBrains IDE +.idea/ + +# Unit test reports +TEST*.xml + +# Generated by MacOS +.DS_Store + +# Generated by Windows +Thumbs.db + +# Applications +*.app +*.exe +*.war + +# Large media files +*.mp4 +*.tiff +*.avi +*.flv +*.mov +*.wmv + diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitmodules b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitmodules new file mode 100644 index 0000000..fbdd6d2 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/.gitmodules @@ -0,0 +1,6 @@ +[submodule "qbrobotics-driver"] + path = qbrobotics-driver + url = https://support-qb@bitbucket.org/qbrobotics/qbrobotics-driver.git +[submodule "serial"] + path = serial + url = https://bitbucket.org/qbrobotics/serial.git diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CHANGELOG.rst b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CHANGELOG.rst new file mode 100644 index 0000000..2ee21aa --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CHANGELOG.rst @@ -0,0 +1,32 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package qbdevice-api-7.x.x +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.1.3 (2022-07-18) +------------------ +* Bug fixed in qbrobotics-driver pkg with installation directory + +1.1.2 (2022-07-15) +------------------ +* Increased timeout in qbrobotics-driver pkg to improve device scanning on Windows PCs + +1.1.1 (2022-07-14) +------------------ +* GNUInstallDirs icluded in Serial project as installation paths were not found in some tests. + + +1.1.0 (2022-07-13) +------------------ +* Added set/get hand side parameter for SH2R + +1.0.2 (2022-07-07) +------------------ +* Bug fixed in homing function + +1.0.1 (2022-07-06) +------------------ +* Changed cmake version + +1.0.0 (2020-05-05) +------------------ +* Alarm Bells Began To Ring diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CMakeLists.txt b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CMakeLists.txt new file mode 100644 index 0000000..ae76a23 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/CMakeLists.txt @@ -0,0 +1,7 @@ +cmake_minimum_required(VERSION 3.5.1) +project(qbrobotics_device_API VERSION 1.1.3 LANGUAGES CXX) +message("-- [ ${PROJECT_NAME}] start compiling") + +# Add dependencies in the order you want them to build. +add_subdirectory(serial) +add_subdirectory(qbrobotics-driver) diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/README.md b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/README.md new file mode 100644 index 0000000..4de3f25 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/README.md @@ -0,0 +1,35 @@ +# qbdevice API + +C++ API to control qbrobotics devices (SoftHand Industry, SoftHand Research, SoftHand2 Research and qbmove) + +## Get this project +In order to clone this project and all its submodules, execute the following command: +``` +git clone --recurse-submodules https://bitbucket.org/qbrobotics/qbdevice-api-7.x.x.git +``` + +## INSTALLATION + +### Research devices +In order to compile the libraries (static or dynamic) to be used in external projects, it is necessary to build both __Serial__ and __qbrobotics-driver__ projects. + +Execute the following commands, in order to compile the project: +``` +mkdir build +cd build +cmake .. +make +``` +Now the **build** folder should contain: +- **qbrobotics-driver-internal** folder, with libqbrobotics_driver.a or libqbrobotics_driver.so library; +- **serial** folder, with libSerial.a or libSerial.so library. + +Use them to exploit qbrobotics API in external projects. + +--- +**NOTE** + +In order to compile **STATIC** or **DYNAMIC** libraries just move to each of CMakeLists.txt files contained inside **qbrobotics-driver-internal** and **serial** folders and edit the **add_library** line in that file. + +--- + diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/.gitignore b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/.gitignore new file mode 100644 index 0000000..e04fcb0 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/.gitignore @@ -0,0 +1,6 @@ +.idea/ +.vscode/ + +*.pro.* +*.autosave +build/ diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CHANGELOG.rst b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CHANGELOG.rst new file mode 100644 index 0000000..2cf9aef --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CHANGELOG.rst @@ -0,0 +1,23 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package qbrobotics_driver +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +7.1.2 (2022-07-18) +------------------ +* Bug fixed with installation directory + +7.1.1 (2022-07-15) +------------------ +* Increased timeout to improve device scanning on Windows PCs + +7.1.0 (2022-07-13) +------------------ +* Added compatibility with SH2R Left + +7.0.2 (2020-05-05) +------------------ +* Bug fix in homing function + +0.0.1 (2020-05-05) +------------------ +* Here be Dragons diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CMakeLists.txt b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CMakeLists.txt new file mode 100644 index 0000000..438fa2f --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/CMakeLists.txt @@ -0,0 +1,112 @@ +cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) +project(qbrobotics_driver VERSION 7.1.2 LANGUAGES CXX) +get_filename_component(API_PATH ../ ABSOLUTE) +if(EXISTS "${API_PATH}/serial/CMakeLists.txt") + message("-- [ ${PROJECT_NAME}] Serial folder found") +endif() +SET(CMAKE_PREFIX_PATH ${API_PATH}/serial) +message("-- [ ${PROJECT_NAME}] start compiling") +find_package(Serial REQUIRED) +add_library(qbrobotics_driver + STATIC # [SHARED | STATIC] + libs/research/src/qbrobotics_research_api.cpp + libs/research/src/qbrobotics_research_api_wrapper.cpp + libs/research/src/qbsofthand_research_api.cpp + libs/research/src/qbmove_research_api.cpp + libs/research/src/qbsofthand2_research_api.cpp +) +target_include_directories(qbrobotics_driver PUBLIC + $ + $ + $ + "$" +) +target_include_directories(qbrobotics_driver + PUBLIC #TODO: make PRIVATE? + "$" + "$" + PRIVATE + "$" +) +target_link_libraries(qbrobotics_driver PRIVATE + Serial::Serial +) +set_target_properties(qbrobotics_driver PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO +) +add_library(qbrobotics_driver::qbrobotics_driver ALIAS qbrobotics_driver) + +install(DIRECTORY libs/research/include + DESTINATION include +) +install(TARGETS qbrobotics_driver EXPORT qbrobotics_driverTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) +install(EXPORT qbrobotics_driverTargets + FILE qbrobotics_driverTargets.cmake + NAMESPACE qbrobotics_driver:: + DESTINATION lib/cmake/qbrobotics_driver +) + +include(CMakePackageConfigHelpers) +configure_package_config_file(qbrobotics_driverConfig.cmake.in + ${CMAKE_CURRENT_BINARY_DIR}/qbrobotics_driverConfig.cmake + INSTALL_DESTINATION lib/cmake/qbrobotics_driver +) +write_basic_package_version_file(qbrobotics_driverConfigVersion.cmake + VERSION ${qbrobotics_driver_VERSION} + COMPATIBILITY SameMajorVersion +) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/qbrobotics_driverConfig.cmake ${CMAKE_CURRENT_BINARY_DIR}/qbrobotics_driverConfigVersion.cmake + DESTINATION lib/cmake/qbrobotics_driver +) + +# Tests +set(TESTS false) # choose if enabling tests or not +if(TESTS) + enable_testing() + find_package(GTest REQUIRED CONFIG) + + add_executable(qbrobotics_research_api_communication + tests/qbrobotics_research_api_communication.cpp + ) + target_link_libraries(qbrobotics_research_api_communication PRIVATE + qbrobotics_driver::qbrobotics_driver + Serial::Serial + GTest::gtest + GTest::gmock + GTest::gtest_main + ) + set_target_properties(qbrobotics_research_api_communication PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) + add_test(NAME qbrobotics_research_api_communication_tests + COMMAND qbrobotics_research_api_communication + ) + + add_executable(qbrobotics_research_api_wrapper_communication + tests/qbrobotics_research_api_wrapper_communication.cpp + ) + target_link_libraries(qbrobotics_research_api_wrapper_communication PRIVATE + qbrobotics_driver::qbrobotics_driver + Serial::Serial + GTest::gtest + GTest::gmock + GTest::gtest_main + ) + set_target_properties(qbrobotics_research_api_wrapper_communication PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) + add_test(NAME qbrobotics_research_api_wrapper_communication_tests + COMMAND qbrobotics_research_api_wrapper_communication + ) +endif() diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/README.md b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/README.md new file mode 100644 index 0000000..b772633 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/README.md @@ -0,0 +1,17 @@ +# qbrobotics driver + +C++ API to control qbrobotics devices (SoftHand Research, SoftHand2 Research and qbmove) + +## INSTALLATION + +### Research devices +This project has to be used as a __subdirectory__ together with the __Serial__ project. + +Download the qbdevice-api-7.x.x project (downloadable from qbrobotics bitbucket repository) to have the complete project. + +--- +**NOTE** + +In order to compile **STATIC** or **DYNAMIC** libraries just move to each of CMakeLists.txt files contained inside **qbrobotics-driver-internal** and **serial** folders and edit the **add_library** line in that file. + +--- diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbmove_research_api.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbmove_research_api.h new file mode 100644 index 0000000..daf6c94 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbmove_research_api.h @@ -0,0 +1,87 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef QBROBOTICS_DRIVER_QBMOVE_RESEARCH_API_H +#define QBROBOTICS_DRIVER_QBMOVE_RESEARCH_API_H + +#include + +namespace qbrobotics_research_api { + +class qbmoveResearch : public Device { + public: + class Params : public Device::Params { + public: + Params() = default; + ~Params() override = default; + + void initParams(const std::vector ¶m_buffer) override; + }; + + explicit qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + explicit qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params); + ~qbmoveResearch() override = default; + + int computeAndStoreMaximumStiffness(); + int setPositionAndStiffnessReferences(int16_t position, int16_t stiffness); + + int getParamRateLimiter(); + int getParamRateLimiter(uint8_t &rate_limiter); + + int setParamRateLimiter(uint8_t rate_limiter); +}; + +class qbmoveLegacyResearch : public qbmoveResearch { + public: + class Params : public qbmoveResearch::Params { + public: + Params() = default; + ~Params() override = default; + + void initParams(const std::vector ¶m_buffer) override; + }; + + explicit qbmoveLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit qbmoveLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + ~qbmoveLegacyResearch() override = default; + + int getControlReferences(std::vector &control_references) override; + int setMotorStates(bool motor_state) override; + + int getParamEncoderOffsets() override; + int getParamEncoderOffsets(std::vector &encoder_offsets) override; + + int setParameter(uint16_t param_type, const std::vector ¶m_data) override; + int setParamId(uint8_t id) override; + int setParamEncoderOffsets(const std::vector &encoder_offsets) override; + int setParamZeros() override; +}; + +} // namespace qbrobotics_research_api + +#endif // QBROBOTICS_DRIVER_QBMOVE_RESEARCH_API_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api.h new file mode 100644 index 0000000..db5e6cc --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api.h @@ -0,0 +1,858 @@ +/*! \mainpage + * + *
+ * + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2022, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H +#define QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H + +// standard libraries +#include +#include +#include +#include +#include +// project libraries +#include +#include + +namespace qbrobotics_research_api { + +/** + * @brief Class that handles the communication with 7.X.X firmware devices + * + */ +class Communication { + public: + /** + * @brief Structure containing information about the type of device connected + * + */ + struct ConnectedDeviceInfo { + uint8_t id {0}; + std::string serial_number {}; + std::string type {}; + std::string sub_type {}; + }; + + Communication(); + explicit Communication(uint32_t baud_rate); + explicit Communication(const serial::Serial::Timeout &timeout); + explicit Communication(uint32_t baud_rate, const serial::Serial::Timeout &timeout); + virtual ~Communication() = default; + + virtual int listConnectedDevices(); + /** + * @brief List all the device id connected to the serial port + * + * @param serial_port_name serial port name + * @param[out] device_ids vector of ConnectedDeviceInfo + * @return number of devices retrieved + */ + virtual int listConnectedDevices(const std::string &serial_port_name, std::vector &device_ids); + /** + * @brief List all the serial ports with a qbrobotics device connected + * + * @param serial_ports_info vector of serial::PortInfo + * @return number of ports retrieved + */ + virtual int listSerialPorts(std::vector &serial_ports_info); + + /** + * @brief Close the serial port if it belongs to the opened port set + * + * @param serial_port_name serial port to close + * @return 0 on success, -1 on error + */ + virtual int closeSerialPort(const std::string &serial_port_name); + + /** + * @brief put in the class map \p serial_ports_ a newly created shared pointer Serial class that provides a portable serial port interface. + * This method use default baud rate and timeout. + * + * @param serial_port_name + */ + virtual int createSerialPort(const std::string &serial_port_name); + + /** + * @brief put in the class map \p serial_ports_ a newly created shared pointer Serial class that provides a portable serial port interface. + * This method use default timeout. + * + * @param serial_port_name + * @param baud_rate + */ + virtual int createSerialPort(const std::string &serial_port_name, uint32_t baud_rate); + + /** + * @brief put in the class map \p serial_ports_ a newly created shared pointer Serial class that provides a portable serial port interface. + * This method use default baud rate. + * + * @param serial_port_name + * @param timeout + */ + virtual int createSerialPort(const std::string &serial_port_name, const serial::Serial::Timeout &timeout); + + /** + * @brief put in the class map \p serial_ports_ a newly created shared pointer Serial class that provides a portable serial port interface. + * + * @param serial_port_name + * @param baud_rate + * @param timeout + */ + virtual int createSerialPort(const std::string &serial_port_name, uint32_t baud_rate, const serial::Serial::Timeout &timeout); + + /** + * Open the serial communication on the given serial port. + * \param serial_port_name the serial port name + * \return 0 on success, -1 on error + */ + virtual int openSerialPort(const std::string &serial_port_name); + + /** + * Open the serial communication on the given serial port with the default timeout. + * \param serial_port_name the serial port name + * \param baud_rate + * \return 0 on success, -1 on error + */ + virtual int openSerialPort(const std::string &serial_port_name, uint32_t baud_rate); + + /** + * Open the serial communication on the given serial port with the default baudate. + * \param serial_port_name the serial port name + * \param timeout + * \return 0 on success, -1 on error + */ + virtual int openSerialPort(const std::string &serial_port_name, serial::Serial::Timeout &timeout); + + /** + * Open the serial communication on the given serial port. + * \param serial_port_name the serial port name + * \param baud_rate + * \param timeout + * \return 0 on success, -1 on error + */ + virtual int openSerialPort(const std::string &serial_port_name, uint32_t baud_rate, serial::Serial::Timeout &timeout); + + /* + ----------------- + parse package, send commands and other utilities to control qbrobotics devices + ----------------- + */ + + virtual int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command); + virtual int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector &data_in); + virtual int sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command); + virtual int sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector &data_in); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, std::vector &data_in); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out, std::vector &data_in); + virtual int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out, std::vector &data_in); + virtual int sendCommandBroadcast(const std::string &serial_port_name, uint8_t command); + virtual int sendCommandBroadcast(const std::string &serial_port_name, uint8_t command, const std::vector &data_out); + //WARN: broadcast should be used only for non-returning methods (it is unreliable for returning methods) + + virtual int deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command); + virtual int deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command, std::vector &data); + virtual int readPackage(const std::string &serial_port_name, std::vector &package_in); + virtual int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command); + virtual int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector &data); + virtual int serializePackage(uint8_t device_id, uint8_t command, std::vector &package_out); + virtual int serializePackage(uint8_t device_id, uint8_t command, const std::vector &data, std::vector &package_out); + virtual int writePackage(const std::string &serial_port_name, const std::vector &package_out); + virtual int writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command); + virtual int writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data); + + virtual uint8_t checksum(const std::vector &data, uint32_t size); + + template + static void swapBytes(T &x) { + auto a = reinterpret_cast(std::addressof(x)); + for(auto b = a+sizeof(T)-1; a + static void swapBytes(std::vector &vector) { + for (auto &&item : vector) { // auto && is required by std::vector which returns a prvalueL + swapBytes(item); + } + } + template + static std::vector vectorCast(std::vector vector) { + auto data_in = reinterpret_cast(vector.data()); + std::vector data_out(data_in, data_in+vector.size()*sizeof(T)/sizeof(S)); + return data_out; + } + template + static std::vector vectorCastAndSwap(std::vector vector) { + auto data_out = vectorCast(vector); + swapBytes(data_out); + return data_out; + } + template + static std::vector vectorSwapAndCast(std::vector vector) { + swapBytes(vector); + return vectorCast(vector); + } + + std::map> getSerialPorts() { return serial_ports_; } + + protected: + explicit Communication(const Communication &communication); + explicit Communication(const Communication &communication, const serial::Serial::Timeout &timeout); + + std::map> connected_devices_; + std::map serial_ports_info_; + std::map> serial_ports_; + + uint32_t serial_ports_baud_rate_; + serial::Serial::Timeout serial_ports_timeout_; + + bool isInSerialPorts(const std::string &serial_port_name); + bool isInSerialPortsInfo(const std::string &serial_port_name); +}; + +/** + * @brief Communication class with a few fix for 6.X.X firmware devices + * + */ +class CommunicationLegacy : public Communication { + public: + CommunicationLegacy() = default; + explicit CommunicationLegacy(const Communication &communication); + explicit CommunicationLegacy(const Communication &communication, const serial::Serial::Timeout &timeout); + virtual ~CommunicationLegacy() = default; + + int parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector &data_in) override; + int sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out, std::vector &data_in) override; + + int deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command, std::vector &data) override; + int readLongPackage(const std::string &serial_port_name, std::vector &package_in); + int readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector &data) override; + int readPackage(const std::string &serial_port_name, std::vector &package_in) override; +}; + +/** + * @brief General class that allow to get/set parameters from/to qbrobotics devices + * + */ +class Device { + public: + /** + * @brief Manage qbrobotics devices parameters + * + */ + class Params { + public: + Params() = default; + virtual ~Params() = default; + + virtual void initParams(const std::vector ¶m_buffer); + + template + static void getParameter(uint8_t param_id, const std::vector ¶m_buffer, std::vector ¶m_vector) { + auto number_of_values = static_cast(param_buffer.at((param_id-1)*PARAM_BYTE_SLOT + 2)); + auto param_iter = param_buffer.begin() + (param_id-1)*PARAM_BYTE_SLOT + 3; + std::vector param_bytes(param_iter, param_iter + number_of_values*sizeof(T)); + param_vector = Communication::vectorCastAndSwap(param_bytes); + } + template + static void getParameter(uint8_t param_id, const std::vector ¶m_buffer, T ¶m) { + std::vector param_vector; + getParameter(param_id, param_buffer, param_vector); + param = param_vector.front(); + } + + std::string serial_number {}; + std::string type {}; + std::string sub_type {}; + + uint8_t id {0}; + std::vector position_pid; + std::vector current_pid; + uint8_t startup_activation {false}; // should be bool + uint8_t input_mode {0}; + uint8_t control_mode {0}; + std::vector encoder_resolutions; + std::vector encoder_offsets; + std::vector encoder_multipliers; + uint8_t use_position_limits {false}; // should be bool + std::vector position_limits; + std::vector position_max_steps; + int16_t current_limit {0}; + uint8_t rate_limiter {0}; // param id after softhand params + }; + + explicit Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + explicit Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params); + virtual ~Device() = default; + + virtual int ping(); + + /** + * @brief Get the reference command sent to the motor(s) of the device. + * + * @param[out] control_references[tick] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getControlReferences(std::vector &control_references); + + /** + * @brief Get the device current(s) absorbed by the motor(s) + * + * @param[out] currents[mA] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getCurrents(std::vector ¤ts); + + /** + * @brief Get the device currents absorbed by the motor(s) and its/their position(s) + * + * @param[out] currents[mA] vector passed by reference + * @param[out] positions[tick] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getCurrentsAndPositions(std::vector ¤ts, std::vector &positions); + + /** + * @brief Get the Positions given by the encoders mounted on the device + * + * @param[out] positions[tick] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getPositions(std::vector &positions); + + /** + * @brief Get the motor(s) velocitie(s) + * + * @param[out] velocities[tick/s] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getVelocities(std::vector &velocities); + + /** + * @brief Get the motor(s) acceleration(s) + * + * @param[out] accelerations[tick/s^2] vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getAccelerations(std::vector &accelerations); + + /** + * @brief Get the device motor state + * + * @param[out] motor_state boolean passed by reference + * @return 0 on success, -1 on error + */ + virtual int getMotorStates(bool &motor_state); + virtual int getCycleTime(int16_t &cycle_time); + + /** + * @brief Set motor(s) control reference - position[tick] + * + * @param control_references[tick] + * @return 0 on success, -1 on error + */ + virtual int setControlReferences(const std::vector &control_references); + + /** + * @brief Set motor(s) control reference and wait for acknowledge - position[tick](only for legacy qbmoves) + * + * @param control_references[tick] + * @return 0 on success, -1 on error + */ + virtual int setControlReferencesAndWait(const std::vector &control_references); + + /** + * @brief Activate or deactivate the motor(s) + * + * @param motor_state the desired motor(s) state(s) + * @return 0 on success, -1 on error + */ + virtual int setMotorStates(bool motor_state); + + /** + * @brief Get all system information + * + * @param[out] info std::string passed by reference + * @return 0 on success, -1 on error + */ + virtual int getInfo(std::string &info); + + /** + * @brief Get the Info of the device + * + * @param info_type - 0 All system information; - 1 Cycles information; - 2 Read Firmware Parameters from SD file; - 3 Read Usage Data from SD file. + * @param info[out] std::string passed by reference + * @return 0 on success, -1 on error + */ + virtual int getInfo(uint16_t info_type, std::string &info); + + /** + * @brief Get the parameters from the device + * + * @param[out] param_buffer vector passed by reference + * @return 0 on success, -1 on error + */ + virtual int getParameters(std::vector ¶m_buffer); + + /** + * @brief Get the parameters from the device with ID \p id + * + * @param[out] id device ID + * @param[out] param_buffer vector passed by reference + * @return 0 on success, -1 on error + * + * \sa getParameters(uint8_t id, std::vector ¶m_buffer) + */ + virtual int getParameters(uint8_t id, std::vector ¶m_buffer); + + /** + * @brief Update the device ID in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamId(); + + /** + * @brief Get the device ID + * + * @param[out] id device ID + * @return 0 on success, -1 on error + * + * \sa getParameters(uint8_t id, std::vector ¶m_buffer) + */ + virtual int getParamId(uint8_t &id); + + /** + * @brief Update the device position PID parameters in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamPositionPID(); + + /** + * @brief Get the device position PID parameters + * + * @param[out] position_pid + * @return 0 on success, -1 on error + */ + virtual int getParamPositionPID(std::vector &position_pid); + + /** + * @brief Update the device current PID parameters in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamCurrentPID(); + + /** + * @brief Get the device current PID parameters + * + * @param[out] current_pid + * @return 0 on success, -1 on error + */ + virtual int getParamCurrentPID(std::vector ¤t_pid); + + /** + * @brief Update the startup activation parameter in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamStartupActivation(); + + /** + * @brief Get the startup activation parameter + * + * @param[out] startup_activation + * @return 0 on success, -1 on error + */ + virtual int getParamStartupActivation(uint8_t &startup_activation); + + /** + * @brief Get the input mode parameter in the class variable \p param_. These values are: + * - 0 - References through external commands + * - 1 - Encoder 3 drives all inputs + * - 2 - Use EMG measure to proportionally drive the position of the motor 1 + * - 3 - Use 2 EMG signals to drive motor position + * - 4 - Use 2 EMG. First reaching threshold wins and its value defines hand closure + * - 5 - Use 2 EMG. First reaching threshold wins and its value defines hand closure. Wait for both EMG to lower under threshold + * + * @return 0 on success, -1 on error + */ + virtual int getParamInputMode(); + + /** + * @brief Get the input parameter to the correspective value of the input mode. + * @param[out] input_mode The possible values are: + * - 0 - References through external commands + * - 1 - Encoder 3 drives all inputs + * - 2 - Use EMG measure to proportionally drive the position of the motor 1 + * - 3 - Use 2 EMG signals to drive motor position + * - 4 - Use 2 EMG. First reaching threshold wins and its value defines hand closure + * - 5 - Use 2 EMG. First reaching threshold wins and its value defines hand closure. Wait for both EMG to lower under threshold + * + * @return 0 on success, -1 on error + */ + virtual int getParamInputMode(uint8_t &input_mode); + + /** + * @brief Update the device control mode in the class variable \p param_ + * - 0 - Classic position control + * - 1 - Direct PWM value + * - 2 - Current control + * - 3 - Position and current control + * - 4 - Deflection control (only for qbmoves) + * - 5 - Deflection and current control (only for qbmoves) + * @return 0 on success, -1 on error + */ + virtual int getParamControlMode(); + + /** + * @brief Get the control mode parameter + * + * @param[out] control_mode + * - 0 - Classic position control + * - 1 - Direct PWM value + * - 2 - Current control + * - 3 - Position and current control + * - 4 - Deflection control (only for qbmoves) + * - 5 - Deflection and current control (only for qbmoves) + * @return 0 on success, -1 on error + */ + virtual int getParamControlMode(uint8_t &control_mode); + + /** + * @brief Update the device encoder resolutions in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderResolutions(); + + /** + * @brief Get the encoder resolution parameters + * + * @param[out] encoder_resolutions + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderResolutions(std::vector &encoder_resolutions); + + /** + * @brief Update the device encoder offsets in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderOffsets(); + + /** + * @brief Get the encoder offsets parameters + * + * @param[out] encoder_offsets + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderOffsets(std::vector &encoder_offsets); + + /** + * @brief Update the device encoder multipliers in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderMultipliers(); + + /** + * @brief Get the encoder multipliers parameters + * + * @param[out] encoder_multipliers + * @return 0 on success, -1 on error + */ + virtual int getParamEncoderMultipliers(std::vector &encoder_multipliers); + + /** + * @brief Update the the use of position limits in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamUsePositionLimits(); + + /** + * @brief Get the use of position limits parameter + * + * @param[out] use_position_limits 0 false, 1 true + * @return 0 on success, -1 on error + */ + virtual int getParamUsePositionLimits(uint8_t &use_position_limits); + + /** + * @brief Update the position limits parameters in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamPositionLimits(); + + /** + * @brief Get position limits parameters + * + * @param[out] position_limits + * @return 0 on success, -1 on error + */ + virtual int getParamPositionLimits(std::vector &position_limits); + + /** + * @brief Update the max step position parameters in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamPositionMaxSteps(); + + /** + * @brief Get max steps position parameters + * + * @param[out] position_max_steps + * @return 0 on success, -1 on error + */ + virtual int getParamPositionMaxSteps(std::vector &position_max_steps); + + /** + * @brief Update the current limits parameters in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + virtual int getParamCurrentLimit(); + + /** + * @brief Get current limit parameter + * + * @param[out] current_limit[mA] + * @return 0 on success, -1 on error + */ + virtual int getParamCurrentLimit(int16_t ¤t_limit); + + /** + * @brief Set the Parameter specified by \p param_type with the values stored in \p param_data + * + * @param param_type ID of the parameter to be set + * @param param_data value(s) to be set + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParameter(uint16_t param_type, const std::vector ¶m_data); + + /** + * @brief Set the ID of the device + * + * @param id desired device ID + * @return 0 on success, < 0 on error + */ + virtual int setParamId(uint8_t id); + + /** + * @brief Set the position PID parameters of the device + * + * @param position_pid desired device PID(position) parameters + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamPositionPID(const std::vector &position_pid); + + /** + * @brief Set the current PID parameters of the device + * + * @param current_pid desired device PID(current) parameters + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamCurrentPID(const std::vector ¤t_pid); + + /** + * @brief Set the startup activation parameter of the device + * + * @param startup_activation + * @return 0 on success, < 0 on error + */ + virtual int setParamStartupActivation(bool startup_activation); + + /** + * @brief Set the input mode parameter of the device + * + * @param input_mode + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamInputMode(uint8_t input_mode); + + /** + * @brief Set the control mode parameter of the device + * + * @param control_mode + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamControlMode(uint8_t control_mode); + + /** + * @brief Set the encoder resolutions parameters of the device + * + * @param encoder_resolutions + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamEncoderResolutions(const std::vector &encoder_resolutions); + + /** + * @brief Set the encoder offsets parameters of the device + * + * @param encoder_offsets + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamEncoderOffsets(const std::vector &encoder_offsets); + + /** + * @brief Set the encoder multipliers parameters of the device + * + * @param encoder_multipliers + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamEncoderMultipliers(const std::vector &encoder_multipliers); + + /** + * @brief Enable or disable the use of position limits + * + * @param use_position_limits + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamUsePositionLimits(bool use_position_limits); + + /** + * @brief Set the position limits parameters of the device + * + * @param position_limits + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamPositionLimits(const std::vector &position_limits); + + /** + * @brief Set the position max steps parameters of the device + * + * @param position_max_steps + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamPositionMaxSteps(const std::vector &position_max_steps); + + /** + * @brief Set the cerrent limit parameter of the device + * + * @param current_limit + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamCurrentLimit(int16_t current_limit); + + /** + * @brief Set motor(s) zero(s) to actual encoder reading + * @return 0 on success, -1 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamZeros(); + + /** + * @brief Set the device baud rate + * + * @param prescaler_divider + * @return 0 on success, -1 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setParamBaudrate(uint8_t prescaler_divider); + + /** + * @brief Restore factory parameter on 7.X.X firmware devices. For 6.X.X it executes INIT_MEM command + * + * @return 0 on success, <0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int restoreFactoryDataMemory(); + + /** + * @brief Restore user parameter on 7.X.X firmware devices. + * \warning not use it in 6.X.X devices. + * + * @return 0 on success, <0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int restoreUserDataMemory(); + + /** + * @brief Store the changed parameters on 7.X.X firmware devices in factory memory. + * \warning not use it in 6.X.X devices. + * + * @return 0 on success, <0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int storeFactoryDataMemory(); + + /** + * @brief Store the changed parameters on 7.X.X firmware devices in user memory. + * \warning not use it in 6.X.X devices. + * + * @return 0 on success, <0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int storeUserDataMemory(); + + /** + * @brief Set the bootloader mode + * + * @return 0 on success, <0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + virtual int setBootloaderMode(); + + std::shared_ptr getParams() { return params_; } + bool isQbmove() { return params_->type == "001"; } + bool isSHPRO() { return params_->type == "006" && params_->sub_type == "001"; } + bool isSH2M() { return params_->type == "006" && params_->sub_type == "020"; } + + protected: + std::string name_; + std::string serial_port_; + std::shared_ptr params_; + std::shared_ptr communication_; +}; + +} // namespace qbrobotics_research_api + +#endif // QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_API_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api_wrapper.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api_wrapper.h new file mode 100644 index 0000000..62d7dac --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_api_wrapper.h @@ -0,0 +1,324 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef QBROBOTICS_RESEARCH_API_WRAPPER_H +#define QBROBOTICS_RESEARCH_API_WRAPPER_H + +// standard libraries +#include +#ifdef _WIN32 +#include +#endif +// project libraries +#include +#include +#include + +typedef struct comm_settings { +#ifndef _WIN32 + int file_handle = -1; + #define INVALID_HANDLE_VALUE -1 +#else + HANDLE file_handle = INVALID_HANDLE_VALUE; +#endif + std::string serial_port_name; +} comm_settings; + +/*** + * Open the given serial port with common settings for qbrobotics devices. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param serial_port_name The serial port path. + * \param BAUD_RATE This old parameter is no longer used. The baud rate is always set to 2M. + */ +void openRS485(comm_settings *handle, const char *serial_port_name, int /*BAUD_RATE*/ = 2000000); + +/*** + * Close the given serial port. + * \param handle A \p comm_settings structure containing info about the communication settings. + */ +void closeRS485(comm_settings *handle); + +/*** + * \sa RS485GetInfo() + */ +void RS485GetInfo(comm_settings *handle, char *buffer); + +/*** + * List the device IDs connected to the given serial port. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param list_of_ids An array of bytes filled with the connected device IDs. + * \return The number of devices connected. + */ +int RS485ListDevices(comm_settings *handle, char list_of_ids[255]); + +/*** + * List the available serial ports (a maximum of 60 ports can be found). + * \param list_of_ports An array of C strings filled with the serial ports paths, e.g. "/dev/ttyUSB0". + * \return The number of serial ports found on success, or \p -1 otherwise. + */ +int RS485listPorts(char list_of_ports[60][255]); + +/*** + * Read a package from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param package A buffer of bytes where the package is stored. + * \return The package length on success, \p -1 otherwise. + */ +int RS485read(comm_settings *handle, int id, char *package); + +/*** + * Activates or deactivates the motor/s of the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param activate \p true to turn motors on, \p false to turn them off. + */ +void commActivate(comm_settings *handle, int id, char activate); + +/*** + * Send the given device board in bootloader mode in order to update its firmware. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \return \p 0 on success, \p -1 otherwise. + */ +int commBootloader(comm_settings *handle, int id); + +/*** + * Calibrate the maximum stiffness value of the given device (the value is stored on the device memory). + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID + * \return \p 0 on success, \p -1 otherwise. + */ +int commCalibrate(comm_settings *handle, int id); + +/*** + * Get encoder acceleration measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param accelerations Acceleration measurements relative to the device encoders. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetAccelerations(comm_settings *handle, int id, short int *accelerations); + +/*** + * Get the activation status of the motor/s of the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param activate \p true if motors are on, \p false otherwise. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetActivate(comm_settings *handle, int id, char *activate); + +/*** + * Get motor current measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param currents Current measurements relative to the device motors. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetCurrents(comm_settings *handle, int id, short int *currents); + +/*** + * Get motor currents and encoder position measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param values Motor current and encoder position measurements vector (currents are in first two places). + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetCurrAndMeas(comm_settings *handle, int id, short int *values); + +/*** + * Get measurements from ElectroMyoGraphic sensors connected to the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param emgs EMG sensors measurements. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetEmg(comm_settings *handle, int id, short int *emgs); + +/*** + * Get internal information about the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param info_type The type of information to be retrieved. + * \param info A buffer that stores the parameter values in a readable format (the minimum buffer size depends on the + * given device; a value of at least 2000 characters is advisable). + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetInfo(comm_settings *handle, int id, short int info_type, char *info); + +/*** + * Get the reference commands from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param inputs The reference command vector which is parsed w.r.t. the actual control mode. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetInputs(comm_settings *handle, int id, short int *inputs); + +/*** + * Get joystick measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param joystick Joystick analog measurements. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetJoystick(comm_settings *handle, int id, short int *joystick); + +/*** + * Get encoder position measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param positions Position measurements relative to the device encoders. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetMeasurements(comm_settings *handle, int id, short int *positions); + +/*** + * Get all the parameters that are stored in the given device memory (\p index must be \p 0 and \p buffer not \p null), + * or set one of them if specified (\p index must be positive and \p values not \p null). + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param index The parameter index w.r.t. the device memory map (unfortunately this is not always specified); use \p 0 + * to retrieve all the parameters in \p buffer, and positive values to set a single parameter with the given \p values. + * \param values An array with the parameter values to be set. + * \param value_size The byte size of the parameter to be set, e.g. 2 for uint16_t, 4 for float, ... + * \param num_of_values The parameter values array size + * \param buffer A buffer of bytes where the parameters are stored. The new v7.x.x C++ API provides a helpful utility + * method to parse a given parameter from the buffer (\sa Device::Params::getParameter). + */ +int commGetParamList(comm_settings *handle, int id, unsigned short index, void *values, unsigned short value_size, unsigned short num_of_values, uint8_t *buffer); + +/*** + * Get encoder speed measurements from the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param velocities Velocity measurements relative to the device encoders. + * \return \p 0 on success, \p -1 otherwise. + */ +int commGetVelocities(comm_settings *handle, int id, short int *velocities); + +/*** + * Start a series of full opening and full closure cycles on the given device. The cycles can also be stopped by + * passing both the \p speed and the \p repetitions equal \p -1. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param speed The speed of openings and closures in range [0, 200]. + * \param repetitions The number of cycles (max value is 32767). + */ +int commHandCalibrate(comm_settings *handle, int id, short int speed, short int repetitions); +/*** + * Initialize the given device memory with the default factory parameters hardcoded in the firmware. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + */ +int commInitMem(comm_settings *handle, int id); + +/*** + * Ping the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \return \p 0 on success, \p -1 otherwise. + */ +int commPing(comm_settings *handle, int id); + +/*** + * Restore the parameter values from the memory archive to the given device memory. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + */ +int commRestoreParams(comm_settings *handle, int id); + +/*** + * Set the device communication baud rate. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param baudrate \p 0 to set 2Mbaud, \p 1 to set 460.8kbaud (actually only 2M is currently supported). + */ +void commSetBaudRate(comm_settings *handle, int id, short int baudrate); + +/*** + * Send reference inputs to the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param flag If nonzero activates the Cuff driving functionality of the board. + */ +void commSetCuffInputs(comm_settings *handle, int id, int flag); + +/*** + * Send reference commands to the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param inputs The reference command vector which is parsed w.r.t. the actual control mode (the references can either + * be positions or currents, or whatever is used to drive the device). + */ +void commSetInputs(comm_settings *handle, int id, short int *inputs); + +int commSetInputsAck(comm_settings *handle, int id, short int *inputs); + +/*** + * Send shaft position and stiffness preset reference commands to the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param inputs The reference command vector where the first element is the shaft position and the second one is the + * stiffness preset. Both values are expressed in motor ticks. + */ +void commSetPosStiff(comm_settings *handle, int id, short int *inputs); + +/*** + * Set the watchdog timer of the given device. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param wdt The watchdog timer in range [0, 500]csec (\p 0 means disabled). + */ +void commSetWatchDog(comm_settings *handle, int id, short int wdt); + +/*** + * Set the actual positions as the zero encoder position values which remain stored the given device memory. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + * \param value This old parameter is no longer used. + * \param num_of_values This old parameter is no longer used. + */ +int commSetZeros(comm_settings *handle, int id, void *values, unsigned short num_of_values); + +/*** + * Store all the actual parameters in the given device memory; these values will be loaded from memory at every startup. + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + */ +int commStoreParams(comm_settings *handle, int id); + +/*** + * Store all the actual parameters in the given device memory archive; these values will not be loaded from memory at + * every startup, but the can be restored by calling \p commRestoreParams(). + * \param handle A \p comm_settings structure containing info about the communication settings. + * \param id The device ID. + */ +int commStoreDefaultParams(comm_settings *handle, int id); + +#endif // QBROBOTICS_RESEARCH_API_WRAPPER_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_commands.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_commands.h new file mode 100644 index 0000000..8314440 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbrobotics_research_commands.h @@ -0,0 +1,189 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_COMMANDS_H +#define QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_COMMANDS_H + +#define API_VERSION "7.1.0" + +#define PARAM_BYTE_SLOT 50 // Number of bytes reserved to param's information +#define PARAM_MENU_SLOT 150 // Number of bytes reserved to a param menu + +/** \name qbrobotics device information types */ +/** \{ */ +#define INFO_ALL 0 ///< All system information +#define INFO_READING 1 ///< Cycles information +#define GET_SD_PARAM 2 ///< Read Firmware Parameters from SD file +#define GET_SD_DATA 3 ///< Read Usage Data from SD file +/** \} */ + +/** \name qbrobotics device commands + * \{ */ +enum qbmove_command { + CMD_PING = 0, ///< Asks for a ping message + CMD_SET_ZEROS = 1, ///< Command for setting the encoders zero position + CMD_STORE_PARAMS = 3, ///< Stores all parameters in memory and loads them + CMD_STORE_DEFAULT_PARAMS = 4, ///< Store current parameters as factory parameters + CMD_RESTORE_PARAMS = 5, ///< Restore default factory parameters + CMD_GET_INFO = 6, ///< Asks for a string of information about + + CMD_SET_VALUE = 7, ///< Not Used + CMD_GET_VALUE = 8, ///< Not Used + + CMD_BOOTLOADER = 9, ///< Sets the bootloader modality to update the firmware + CMD_INIT_MEM = 10, ///< Initialize the memory with the default values + CMD_CALIBRATE = 11, ///< Starts the stiffness calibration of the qbMove + CMD_GET_PARAM_LIST = 12, ///< Command to get the parameters list or to set a defined value chosen by the use + CMD_HAND_CALIBRATE = 13, ///< Starts a series of opening and closures of the hand + + CMD_ACTIVATE = 128, ///< Command for activating/deactivating the device + CMD_GET_ACTIVATE = 129, ///< Command for getting device activation state + CMD_SET_INPUTS = 130, ///< Command for setting reference inputs + CMD_GET_INPUTS = 131, ///< Command for getting reference inputs + CMD_GET_MEASUREMENTS = 132, ///< Command for asking device's position measurements + CMD_GET_CURRENTS = 133, ///< Command for asking device's current measurements + CMD_GET_CURR_AND_MEAS = 134, ///< Command for asking device's measurements and currents + CMD_SET_POS_STIFF = 135, ///< Not used in the softhand firmware + CMD_GET_EMG = 136, ///< Command for asking device's emg sensors measurements + CMD_GET_VELOCITIES = 137, ///< Command for asking device's velocity measurements + CMD_GET_COUNTERS = 138, ///< Command for asking device's counters, mostly used for debugging sent commands + CMD_GET_ACCEL = 139, ///< Command for asking device's acceleration measurements + CMD_GET_CURR_DIFF = 140, ///< Command for asking device's current difference between a measured one and + /// an estimated one (only for SoftHand) + CMD_SET_CURR_DIFF = 141, ///< Command used to set current difference modality (only for Cuff device) + CMD_SET_CUFF_INPUTS = 142, ///< Command used to set Cuff device inputs (only for Cuff device) + CMD_SET_WATCHDOG = 143, ///< Command for setting watchdog timer or disable it + CMD_SET_BAUDRATE = 144, ///< Command for setting baud rate of communication + CMD_EXT_DRIVE = 145, ///< Command to set the actual measurements as inputs to another device (only + /// for Armslider device) + CMD_GET_JOYSTICK = 146, ///< Command to get the joystick measurements (only for devices driven by a joystick) + CMD_SET_INPUTS_ACK = 147, ///< Command to set the device inputs and return an acknowledgment signal + /// (needed for less comm. errors) + CMD_SET_SYNERGIES = 148, ///< Command to set the synergies to SoftHand 2 + CMD_GET_SYNERGIES = 149, ///< Command to get the synergies of SoftHand 2 + CMD_GET_CYCLE_TIME = 150 ///< Command to get the device cycle time +}; + +enum additional_command +{ + CMD_GET_IMU_READINGS = 161, ///< Retrieve accelerometers, gyroscopes and magnetometers readings + CMD_GET_IMU_PARAM = 162, ///< Retrieve and set IMU parameters + CMD_GET_ENCODER_CONF = 163, ///< Get encoder configuration + CMD_GET_ENCODER_RAW = 164, ///< Get all encoder raw values + CMD_GET_ADC_CONF = 165, ///< Get ADC configuration + CMD_GET_ADC_RAW = 166 ///< Get ADC raw values +}; +/** \} */ + +/** \name qbrobotics device parameters + * \{ */ +enum qbmove_parameter { + PARAM_ID = 0, ///< Device's ID number + PARAM_PID_CONTROL = 1, ///< PID parameters + PARAM_STARTUP_ACTIVATION = 2, ///< Start up activation byte + PARAM_INPUT_MODE = 3, ///< Input mode + PARAM_CONTROL_MODE = 4, ///< Choose the kind of control between position control, current control, + /// direct PWM value or current+position control + PARAM_MEASUREMENT_OFFSET = 5, ///< Adds a constant offset to the measurements + PARAM_MEASUREMENT_MULTIPLIER = 6, ///< Adds a multiplier to the measurements + PARAM_POS_LIMIT_FLAG = 7, ///< Enable/disable position limiting + PARAM_POS_LIMIT = 8, ///< Position limit values + /// | int32 | int32 | int32 | int32 | + /// | INF_LIM_1 | SUP_LIM_1 | INF_LIM_2 | SUP_LIM_2 | + PARAM_MAX_STEP_POS = 9, ///< Used to slow down movements for positive values + PARAM_MAX_STEP_NEG = 10, ///< Used to slow down movements for negative values + PARAM_POS_RESOLUTION = 11, ///< Angle resolution for inputs and measurements (used during communication) + PARAM_CURRENT_LIMIT = 12, ///< Limit for absorbed current + PARAM_EMG_CALIB_FLAG = 13, ///< Enable calibration on startup + PARAM_EMG_THRESHOLD = 14, ///< Minimum value to have effect + PARAM_EMG_MAX_VALUE = 15, ///< Maximum value of EMG + PARAM_EMG_SPEED = 16, ///< Closure speed when using EMG + PARAM_PID_CURR_CONTROL = 18, ///< PID current control + PARAM_DOUBLE_ENC_ON_OFF = 19, ///< Double Encoder Y/N + PARAM_MOT_HANDLE_RATIO = 20, ///< Multiplier between handle and motor + PARAM_MOTOR_SUPPLY = 21, ///< Motor supply voltage of the hand + PARAM_CURRENT_LOOKUP = 23, ///< Table of values used to calculate an estimated current of the SoftHand + PARAM_DL_POS_PID = 24, ///< Double loop position PID + PARAM_DL_CURR_PID = 25 ///< Double loop current PID +}; +/** \} */ + +enum qbmove_resolution { + RESOLUTION_360 = 0, + RESOLUTION_720 = 1, + RESOLUTION_1440 = 2, + RESOLUTION_2880 = 3, + RESOLUTION_5760 = 4, + RESOLUTION_11520 = 5, + RESOLUTION_23040 = 6, + RESOLUTION_46080 = 7, + RESOLUTION_92160 = 8 +}; + +enum qbmove_input_mode { + INPUT_MODE_EXTERNAL = 0, ///< References through external commands (default) + INPUT_MODE_ENCODER3 = 1, ///< Encoder 3 drives all inputs + INPUT_MODE_EMG_PROPORTIONAL = 2, ///< Use EMG measure to proportionally drive the position of the motor 1 + INPUT_MODE_EMG_INTEGRAL = 3, ///< Use 2 EMG signals to drive motor position + INPUT_MODE_EMG_FCFS = 4, ///< Use 2 EMG. First reaching threshold wins and its value defines hand closure + INPUT_MODE_EMG_FCFS_ADV = 5 ///< Use 2 EMG. First reaching threshold wins and its value defines hand closure + /// Wait for both EMG to lower under threshold +}; + +enum qbmove_control_mode { + CONTROL_ANGLE = 0, ///< Classic position control + CONTROL_PWM = 1, ///< Direct PWM value + CONTROL_CURRENT = 2, ///< Current control + CURR_AND_POS_CONTROL = 3, ///< Position and current control + DEFLECTION_CONTROL = 4, ///< Deflection control + DEFL_CURRENT_CONTROL = 5 ///< Deflection and current control +}; + +enum motor_supply_tipe { + MAXON_24V = 0, + MAXON_12V = 1 +}; + +enum acknowledgment_values { + ACK_ERROR = 0, + ACK_OK = 1 +}; + +enum data_types { + TYPE_FLAG = 0, + TYPE_INT8 = 1, + TYPE_UINT8 = 2, + TYPE_INT16 = 3, + TYPE_UINT16 = 4, + TYPE_INT32 = 5, + TYPE_UINT32 = 6, + TYPE_FLOAT = 7, + TYPE_DOUBLE = 8, + TYPE_STRING = 9 // Custom data type +}; + +#endif // QBROBOTICS_DRIVER_QBROBOTICS_RESEARCH_COMMANDS_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand2_research_api.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand2_research_api.h new file mode 100644 index 0000000..3ae24f9 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand2_research_api.h @@ -0,0 +1,114 @@ +#include + +namespace qbrobotics_research_api { + /** + * @brief SoftHand 2 class that allows to control and get/set parameters. + * + */ + class qbSoftHand2MotorsResearch : public Device { + public: + class Params : public Device::Params { + public: + Params() = default; + ~Params() override = default; + + void initParams(const std::vector ¶m_buffer) override; + + uint8_t hand_side {0}; + }; + + explicit qbSoftHand2MotorsResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit qbSoftHand2MotorsResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + ~qbSoftHand2MotorsResearch() override = default; + + int setMotorStates(bool motor_state) override; + + int setParameter(uint16_t param_type, const std::vector ¶m_data) override; + int setParamId(uint8_t id) override; + int setParamZeros() override; + + + /** + * @brief Update the SoftHand 2 hand_side parameter in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + int getParamHandSide(); + + /** + * @brief Get the hand_side parameter for RIGHT or LEFT Hand for SoftHand 2. + * + * @param[out] hand_side 0 for RIGHT Hand, 1 for LEFT Hand + * @return 0 on success, -1 on error + * + */ + int getParamHandSide(uint8_t &hand_side); + + /** + * @brief Update the SoftHand 2 encoder offsets in the class variable \p param_ + * + * @return 0 on success, -1 on error + */ + int getParamEncoderOffsets() override; + + /** + * @brief Get the encoder resolution parameters + * + * @param[out] encoder_resolutions + * @return 0 on success, -1 on error + */ + int getParamEncoderOffsets(std::vector &encoder_offsets) override; + + /** + * @brief Get the synergies values of the SoftHand 2. (ADDITIVE SINERGIES) + * + * @param[out] synergies + * @return 0 on success, -1 on error + */ + int getSynergies(std::vector &synergies); + + /** + * @brief Set the encoder offsets parameters of the SoftHand 2 + * + * @param encoder_offsets + * @return 0 on success, < 0 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + int setParamEncoderOffsets(const std::vector &encoder_offsets) override; + + /** + * @brief Set the Additive Synergies References to SoftHand 2. This command allows to command the SoftHand closure and the its direction of closure. (ADDITIVE SYNERGIES) + * + * @param synergy_1 qbSoftHand 2 closure + * @param synergy_2 qbSoftHand 2 direction of closure + * @return 0 on success, -1 on error + */ + int setAdditiveSynergiesReferences(int16_t synergy_1, int16_t synergy_2); + + /** + * @brief Set the Multiplicative Synergies References to SoftHand 2. This command allows to command the SoftHand closure and the its direction of closure. (MULTIPLICATIVE SYNERGIES) + * + * @param synergy_1 qbSoftHand 2 closure - [0, 1] + * @param synergy_2 qbSoftHand 2 direction of closure - [-1, 1] + * @return 0 on success, -1 on error, -2 if limits are violated + */ + int setMultiplicativeSynergiesReferences(float synergy_1, float synergy_2); + + /** + * @brief Set the hand_side parameter for RIGHT or LEFT Hand for SoftHand 2. + * + * @param hand_side 0 for RIGHT Hand, 1 for LEFT Hand + * @return 0 on success, -1 on error + * @warning The improper use of this function could damage the device and invalidate the device warranty. Contact our support team (support@qbrobotics.com) for more information. + */ + int setParamHandSide(uint8_t hand_side); + + /** + * @brief Blocking function that moves SoftHand 2 motors to home position + * + * @return 0 on success, -1 on communication error, -2 if the SoftHand 2 returns to home position but not within the defined timeframe. + * + */ + int setHomePosition(); + }; +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand_research_api.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand_research_api.h new file mode 100644 index 0000000..13c93e5 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/include/qbrobotics_research_api/qbsofthand_research_api.h @@ -0,0 +1,118 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef QBROBOTICS_DRIVER_QBSOFTHAND_RESEARCH_API_H +#define QBROBOTICS_DRIVER_QBSOFTHAND_RESEARCH_API_H + +#include + +namespace qbrobotics_research_api { + +class qbSoftHandResearch : public Device { + public: + class Params : public Device::Params { + public: + Params() = default; + ~Params() override = default; + + void initParams(const std::vector ¶m_buffer) override; + + uint8_t use_emg_calibration {false}; + std::vector emg_thresholds; + std::vector emg_max_value; + uint8_t emg_speed {0}; + uint8_t use_double_encoder {false}; + int8_t handle_ratio {0}; + uint8_t use_pwm_rescaling {false}; + std::vector current_lookup_table; + }; + + explicit qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + explicit qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params); + ~qbSoftHandResearch() override = default; + + int openCloseCycles(uint16_t speed, uint16_t cycles); + int getEMGs(std::vector &emg_values); + int getJoystick(std::vector &joystick_values); + + int getParamUseEMGCalibration(); + int getParamUseEMGCalibration(uint8_t &use_emg_calibration); + int getParamEMGThresholds(); + int getParamEMGThresholds(std::vector &emg_thresholds); + int getParamEMGMaxValues(); + int getParamEMGMaxValues(std::vector &emg_max_value); + int getParamEMGSpeed(); + int getParamEMGSpeed(uint8_t &emg_speed); + int getParamUseDoubleEncoder(); + int getParamUseDoubleEncoder(uint8_t &use_double_encoder); + int getParamHandleRatio(); + int getParamHandleRatio(int8_t &handle_ratio); + int getParamUsePWMRescaling(); + int getParamUsePWMRescaling(uint8_t &use_pwm_rescaling); + int getParamCurrentLookupTable(); + int getParamCurrentLookupTable(std::vector ¤t_lookup_table); + int getParamRateLimiter(); + int getParamRateLimiter(uint8_t &rate_limiter); + + int setParamUseEMGCalibration(uint8_t use_emg_calibration); + int setParamEMGThresholds(const std::vector &emg_thresholds); + int setParamEMGMaxValues(const std::vector &emg_max_value); + int setParamEMGSpeed(uint8_t emg_speed); + int setParamUseDoubleEncoder(uint8_t use_double_encoder); + int setParamHandleRatio(int8_t handle_ratio); + int setParamUsePWMRescaling(uint8_t use_pwm_rescaling); + int setParamCurrentLookupTable(const std::vector ¤t_lookup_table); + int setParamRateLimiter(uint8_t rate_limiter); +}; + +class qbSoftHandLegacyResearch : public qbSoftHandResearch { + public: + class Params : public qbSoftHandResearch::Params { + public: + Params() = default; + ~Params() override = default; + +// void initParams(const std::vector ¶m_buffer) override; // not necessary at the moment + }; + + explicit qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id); + explicit qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params); + explicit qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params); + ~qbSoftHandLegacyResearch() override = default; + + // same as qbmoveOldResearch + int setMotorStates(bool motor_state) override; + + int setParameter(uint16_t param_type, const std::vector ¶m_data) override; + int setParamId(uint8_t id) override; + int setParamZeros() override; +}; + +} // namespace qbrobotics_research_api + +#endif // QBROBOTICS_DRIVER_QBSOFTHAND_RESEARCH_API_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/qbapi_research.pro b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/qbapi_research.pro new file mode 100644 index 0000000..9ccd203 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/qbapi_research.pro @@ -0,0 +1,20 @@ +TEMPLATE = lib +CONFIG += c++latest +CONFIG += staticlib +VERSION = 7.0.0 +TARGET = libqbapi-$$VERSION + +#!!! Per compilare è necessario che questo repo (qbrobotics-driver-internal) +# si trovi nella stessa cartella del repo serial (da cui pescare gli header) + +INCLUDEPATH += include +INCLUDEPATH += ../../../serial/include # <-- header per le funzioni serial +DEPENDPATH += include + +SOURCES = src/qbrobotics_research_api.cpp +SOURCES += src/qbmove_research_api.cpp +SOURCES += src/qbsofthand_research_api.cpp + +win32 { + LIBS += -lsetupapi +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbmove_research_api.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbmove_research_api.cpp new file mode 100644 index 0000000..dfcd614 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbmove_research_api.cpp @@ -0,0 +1,162 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +using namespace qbrobotics_research_api; + +void qbmoveResearch::Params::initParams(const std::vector ¶m_buffer) { + Device::Params::initParams(param_buffer); + getParameter(14, param_buffer, rate_limiter); +} + +qbmoveResearch::qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +qbmoveResearch::qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +qbmoveResearch::qbmoveResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::move(params)) {} + +int qbmoveResearch::computeAndStoreMaximumStiffness() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_CALIBRATE, data_in) < 0) { + return -1; + } + return 0; +} + +int qbmoveResearch::setPositionAndStiffnessReferences(int16_t position, int16_t stiffness) { + auto const data_out = Communication::vectorSwapAndCast({position, stiffness}); + if (communication_->sendCommand(serial_port_, params_->id, CMD_SET_POS_STIFF, data_out) < 0) { + return -1; + } + return 0; +} + +int qbmoveResearch::getParamRateLimiter() { + return getParamRateLimiter(params_->rate_limiter); +} + +int qbmoveResearch::getParamRateLimiter(uint8_t &rate_limiter) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(14, param_buffer, rate_limiter); + return 0; +} + +int qbmoveResearch::setParamRateLimiter(uint8_t rate_limiter) { + int set_fail = setParameter(14, Communication::vectorSwapAndCast({rate_limiter})); + if (!set_fail) { + params_->rate_limiter = rate_limiter; + } + return set_fail; +} + + +// ---------------------------------------------------------------- + + +void qbmoveLegacyResearch::Params::initParams(const std::vector ¶m_buffer) { + qbmoveResearch::Params::initParams(param_buffer); + // Legacy firmware needs inverted signs + std::for_each(encoder_offsets.begin(), encoder_offsets.end(), [](int16_t &x){ x *= -1; } ); +} + +qbmoveLegacyResearch::qbmoveLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : qbmoveResearch(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +qbmoveLegacyResearch::qbmoveLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : qbmoveResearch(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +int qbmoveLegacyResearch::setParameter(uint16_t param_type, const std::vector ¶m_data) { + int set_fail = qbmoveResearch::setParameter(param_type, param_data); + if (!set_fail && param_type != 1) { // WARN: the storeUserDataMemory for ID change is called by the specific method + set_fail = qbmoveResearch::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + } + return set_fail; +} + +int qbmoveLegacyResearch::getControlReferences(std::vector &control_references) { + int set_fail = qbmoveResearch::getControlReferences(control_references); + // Legacy firmware needs inverted signs + std::for_each( control_references.begin(), control_references.end(), [](int16_t &x){ x *= -1; } ); + return set_fail; +} + +int qbmoveLegacyResearch::setMotorStates(bool motor_state) { + int set_fail = qbmoveResearch::setMotorStates(motor_state); + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + return set_fail; +} + +int qbmoveLegacyResearch::getParamEncoderOffsets() { + return getParamEncoderOffsets(params_->encoder_offsets); +} + +int qbmoveLegacyResearch::getParamEncoderOffsets(std::vector &encoder_offsets) { + int set_fail = qbmoveResearch::getParamEncoderOffsets(encoder_offsets); + // Legacy firmware needs inverted signs + std::for_each( encoder_offsets.begin(), encoder_offsets.end(), [](int16_t &x){ x *= -1; } ); + return set_fail; +} + +int qbmoveLegacyResearch::setParamId(uint8_t id) { // old method only for legacy devices + uint8_t previous_id = params_->id; + int set_fail = qbmoveResearch::setParamId(id); + if (!set_fail) { + params_->id = previous_id; // WARN: need to send the command to the previous ID on legacy devices! + set_fail = qbmoveResearch::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + params_->id = id; + } + return set_fail; +} + +int qbmoveLegacyResearch::setParamEncoderOffsets(const std::vector &encoder_offsets) { + std::vector legacy_offsets = encoder_offsets; + // Legacy firmware needs inverted signs + legacy_offsets.at(2) *= -1; + return qbmoveResearch::setParamEncoderOffsets(legacy_offsets); +} + +int qbmoveLegacyResearch::setParamZeros() { // old method only for legacy devices + std::vector positions; + if (getPositions(positions)) { + return -1; + } + std::vector offsets; + if (getParamEncoderOffsets(offsets)) { + return -1; + } + + std::transform(offsets.begin(), offsets.end(), positions.begin(), offsets.begin(), std::plus()); // inverted signs for legacy leads to "plus" operation + + return setParamEncoderOffsets(offsets); +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api.cpp new file mode 100644 index 0000000..d2c6f4b --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api.cpp @@ -0,0 +1,1155 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +// standard libraries +#include +#include // for macOS +// project libraries +#include + +using namespace qbrobotics_research_api; + +Communication::Communication() + : Communication(2000000) {} + +Communication::Communication(uint32_t baud_rate) + : Communication(baud_rate, serial::Serial::Timeout(200)) {} // could be serial::Serial::Timeout(20) for new devices but legacy require a higher timeout(even 150 for SHR2) + +Communication::Communication(const serial::Serial::Timeout &timeout) + : Communication(2000000, timeout) {} + +Communication::Communication(uint32_t baud_rate, const serial::Serial::Timeout &timeout) + : serial_ports_baud_rate_(baud_rate), + serial_ports_timeout_(timeout) {} + +Communication::Communication(const Communication &communication) + : Communication(communication, communication.serial_ports_timeout_) {} + +Communication::Communication(const Communication &communication, const serial::Serial::Timeout &timeout) + : connected_devices_(communication.connected_devices_), + serial_ports_info_(communication.serial_ports_info_), + serial_ports_(communication.serial_ports_), + serial_ports_baud_rate_(communication.serial_ports_baud_rate_), + serial_ports_timeout_(timeout) {} + +int Communication::listSerialPorts(std::vector &serial_ports_info) { + serial_ports_info.clear(); + + std::vector connected_serial_ports; + if (serial::getPortsInfo(connected_serial_ports) < 0) { + return -1; + } + + // add new entries to private members + for (auto const &serial_port : connected_serial_ports) { + if (serial_port.manufacturer == "QB Robotics" || serial_port.id_vendor == 0x403) { + serial_ports_info.push_back(serial_port); + if (!isInSerialPortsInfo(serial_port.serial_port)) { + if (isInSerialPorts(serial_port.serial_port)) { + return -1; // this should never happen + } + serial_ports_info_[serial_port.serial_port] = serial_port; + serial_ports_[serial_port.serial_port] = std::make_shared(); // this does not open the serial port + } + } + } + + // remove no longer existing entries in private members + for (auto serial_port_it = serial_ports_info_.begin(); serial_port_it != serial_ports_info_.end();) { + if (std::find_if(connected_serial_ports.begin(), connected_serial_ports.end(), + [&](auto item){ return serial_port_it->first == item.serial_port; }) == connected_serial_ports.end()) { + serial_ports_.erase(serial_port_it->first); + serial_port_it = serial_ports_info_.erase(serial_port_it); + } else { + serial_port_it++; // only when items are not erased + } + } + + return serial_ports_info.size(); +} + +int Communication::listConnectedDevices() { + connected_devices_.clear(); + int connected_devices = 0; + for (auto const &serial_port : serial_ports_) { + std::vector device_ids; + if (listConnectedDevices(serial_port.first, device_ids) > 0) { + connected_devices_[serial_port.first] = device_ids; + connected_devices += device_ids.size(); + } + } + return connected_devices; +} + +int Communication::listConnectedDevices(const std::string &serial_port_name, std::vector &device_ids) { + device_ids.clear(); + + serial::Serial::Timeout short_timeout(serial::Serial::Timeout(5)); + if (!openSerialPort(serial_port_name, serial_ports_baud_rate_, short_timeout)) { + serial_ports_.at(serial_port_name)->setTimeout(short_timeout); // if already connected it might have a different timeout + std::vector data_in; + + std::vector max_id({255, 50, 10, 10, 10}); // max ID checked during i-th scan + // Smart device scan: more retries for lower IDs (more likely) + // 1-10: very likely -> 5x10 scan + // 11-50: unlikely -> 2x40 scan + // 51-255: very unlikely -> 1x205 scan + // = 335 pings + for (size_t scan = 0; scan < max_id.size(); scan++) { + for (uint16_t device_id = 1; device_id <= max_id.at(scan); device_id++) { // should be uint8_t, but uint8_t(256) == 0 and it never ends + // skip device_id if already found + if (std::find_if(device_ids.begin(), device_ids.end(), [&device_id](const auto& info){ return info.id == device_id; }) != device_ids.end()) { + continue; + } + // sendCommandAndParse with 0 repetitions. Actual repetitions are decided by the for loops + if (sendCommandAndParse(serial_port_name, device_id, CMD_PING, 0, data_in) >= 0) { + device_ids.push_back({static_cast(device_id)}); + if (data_in.size() > 0) { // the 4-byte S/N is stored in the ping payload (only for v7.1+ firmware) + std::stringstream str; + str << std::setfill('0') << std::setw(10) << Communication::vectorCastAndSwap(data_in).front(); + std::string serial_number(str.str()); + device_ids.back().serial_number = serial_number; + device_ids.back().type = std::string(serial_number.begin(), serial_number.begin()+3); + device_ids.back().sub_type = std::string(serial_number.begin()+3, serial_number.begin()+6); + } + } + } + } + std::sort(device_ids.begin(), device_ids.end(), [](const auto& a, const auto& b){ return a.id > b.id; }); + } + serial_ports_.at(serial_port_name)->setTimeout(serial_ports_timeout_); + + return device_ids.size(); +} + +int Communication::closeSerialPort(const std::string &serial_port_name) { + if (!isInSerialPorts(serial_port_name)) { + return -1; + } + + try { + serial_ports_.at(serial_port_name)->close(); + } catch (...) { + return -1; + } + + return 0; +} + +int Communication::createSerialPort(const std::string &serial_port_name) { + return createSerialPort(serial_port_name, serial_ports_baud_rate_); +} + +int Communication::createSerialPort(const std::string &serial_port_name, uint32_t baud_rate) { + return createSerialPort(serial_port_name, baud_rate, serial_ports_timeout_); +} + +int Communication::createSerialPort(const std::string &serial_port_name, const serial::Serial::Timeout &timeout) { + return createSerialPort(serial_port_name, serial_ports_baud_rate_, timeout); +} + +int Communication::createSerialPort(const std::string &serial_port_name, uint32_t baud_rate, const serial::Serial::Timeout &timeout) { + if (isInSerialPorts(serial_port_name)) { + return -1; + } + + try { + serial_ports_[serial_port_name] = std::make_shared(serial_port_name, baud_rate, timeout); + } catch (...) { + return -1; + } + return 0; +} + +int Communication::openSerialPort(const std::string &serial_port_name) { + return openSerialPort(serial_port_name, serial_ports_baud_rate_); +} + +int Communication::openSerialPort(const std::string &serial_port_name, uint32_t baud_rate) { + return openSerialPort(serial_port_name, baud_rate, serial_ports_timeout_); +} + +int Communication::openSerialPort(const std::string &serial_port_name, serial::Serial::Timeout &timeout) { + return openSerialPort(serial_port_name, serial_ports_baud_rate_, timeout); +} + +int Communication::openSerialPort(const std::string &serial_port_name, uint32_t baud_rate, serial::Serial::Timeout &timeout) { + if (!isInSerialPorts(serial_port_name)) { + return -1; + } + if (serial_ports_.at(serial_port_name)->isOpen()) { + return 0; + } + + try { + if (serial_ports_.at(serial_port_name)->getPort().empty()) { + serial_ports_.at(serial_port_name)->setPort(serial_port_name); + } + serial_ports_.at(serial_port_name)->setBaudrate(baud_rate); + serial_ports_.at(serial_port_name)->setTimeout(timeout); + serial_ports_.at(serial_port_name)->open(); + } catch (...) { + return -1; + } + return 0; +} + +int Communication::parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command) { + std::vector data_in; + return parsePackage(serial_port_name, device_id, command, data_in); +} + +int Communication::parsePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector &data_in) { + data_in.clear(); + uint8_t device_id_in = 0; + uint8_t command_in = 0; + std::vector data_in_buffer; + + // works only for non-broadcast commands + if (!device_id) { + return -1; + } + + while (readPackage(serial_port_name, device_id_in, command_in, data_in_buffer) > 0) { + //WARN: this is ugly because it can lead to unwanted command parsing on CMD_INIT_MEM, CMD_RESTORE_PARAMS or CMD_GET_PARAM_LIST (especially this that is used for many things!) + if (device_id != device_id_in && (command != CMD_INIT_MEM && command != CMD_RESTORE_PARAMS && (command != CMD_GET_PARAM_LIST || data_in_buffer.size() != 1))) { // id and id_in can differ on CMD_INIT_MEM or CMD_RESTORE_PARAMS (and also with CMD_GET_PARAM_LIST when changing the device ID) + continue; + } + // ACK is checked from the caller that knows whether the command has ACK or not + if (command_in == command) { + data_in = data_in_buffer; + if (device_id != device_id_in) { + data_in.push_back(device_id_in); // ugly fix to pass the new id to the caller (data_in_buffer is only the ACK response in these cases) + } + return data_in.size(); + } + } + return -1; +} + +int Communication::sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command) { + std::vector data_out; + return sendCommand(serial_port_name, device_id, command, data_out); +} + +int Communication::sendCommand(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out) { + if (openSerialPort(serial_port_name)) { + return -1; + } + if (writePackage(serial_port_name, device_id, command, data_out) < 0) { + return -1; + } + return 0; +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command) { + return sendCommandAndParse(serial_port_name, device_id, command, 2); // 3 attempts by default (1 + 2 optional retrials) +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats) { + std::vector data_in, data_out; + return sendCommandAndParse(serial_port_name, device_id, command, max_repeats, data_out, data_in); +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, std::vector &data_in) { + return sendCommandAndParse(serial_port_name, device_id, command, 2, data_in); // 3 attempts by default (1 + 2 optional retrials) +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, std::vector &data_in) { + std::vector data_out; + return sendCommandAndParse(serial_port_name, device_id, command, max_repeats, data_out, data_in); +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out) { + return sendCommandAndParse(serial_port_name, device_id, command, 2, data_out); // 3 attempts by default (1 + 2 optional retrials) +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out) { + std::vector data_in; + return sendCommandAndParse(serial_port_name, device_id, command, max_repeats, data_out, data_in); +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data_out, std::vector &data_in) { + return sendCommandAndParse(serial_port_name, device_id, command, 2, data_out, data_in); // 3 attempts by default (1 + 2 optional retrials) +} + +int Communication::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out, std::vector &data_in) { + uint8_t repeats = 0; + data_in.clear(); + + while (repeats <= max_repeats) { + if (sendCommand(serial_port_name, device_id, command, data_out) < 0) { + repeats++; + continue; + } + + if (command == CMD_STORE_PARAMS || command == CMD_STORE_DEFAULT_PARAMS) { // writing to EEPROM takes a while + std::this_thread::sleep_for(std::chrono::milliseconds(600)); + } + + std::vector packages_in; + if (parsePackage(serial_port_name, device_id, command, packages_in) >= 0) { + data_in = packages_in; + break; + } + + repeats++; + } + + return repeats <= max_repeats ? data_in.size() : -1; +} + +int Communication::sendCommandBroadcast(const std::string &serial_port_name, uint8_t command) { + std::vector data_out; + return sendCommand(serial_port_name, 0, command, data_out); + //WARN: broadcast should be used only for non-returning methods (it is unreliable for returning methods) +} + +int Communication::sendCommandBroadcast(const std::string &serial_port_name, uint8_t command, const std::vector &data_out) { + return sendCommand(serial_port_name, 0, command, data_out); + //WARN: broadcast should be used only for non-returning methods (it is unreliable for returning methods) +} + +int Communication::deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command) { + std::vector data; + return deserializePackage(package_in, device_id, command, data); +} + +int Communication::deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command, std::vector &data) { + if (package_in.size() < 6) { + return -1; + } + if (package_in.at(0) != ':' || package_in.at(1) != ':') { + return -1; + } + if (package_in.at(2) == 0) { + return -1; + } + if (package_in.at(4) == CMD_GET_INFO || package_in.at(4) == CMD_GET_PARAM_LIST) { // special case for very long packets that exceed 8bit length + if (package_in.at(3) != package_in.size()-4 && (static_cast(package_in.at(3)) << 5) != package_in.size()-4) { + return -1; + } + } else { // all common commands + if (package_in.at(3) != package_in.size()-4) { + return -1; + } + } + if (package_in.back() != checksum(package_in, package_in.size()-1)) { + return -1; + } + device_id = package_in.at(2); + command = package_in.at(4); + data.clear(); + data.insert(std::end(data), std::begin(package_in)+5, std::end(package_in)-1); + + return package_in.size(); +} + +int Communication::readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command) { + std::vector package_in; + if (readPackage(serial_port_name, package_in) < 0) { + return -1; + } + return deserializePackage(package_in, device_id, command); +} + +int Communication::readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector &data) { + std::vector package_in; + if (readPackage(serial_port_name, package_in) < 0) { + return -1; + } + return deserializePackage(package_in, device_id, command, data); +} + +int Communication::readPackage(const std::string &serial_port_name, std::vector &package_in) { + package_in.clear(); + try { + serial_ports_.at(serial_port_name)->read(package_in, 4); + if (package_in.size() != 4) { + serial_ports_.at(serial_port_name)->flush(); + return -1; + } + if (package_in.at(0) != ':' || package_in.at(1) != ':') { + serial_ports_.at(serial_port_name)->flush(); + return -1; + } + serial_ports_.at(serial_port_name)->read(package_in, package_in.at(3)); + + // special case for very long packets that exceed 8bit length + if (package_in.size() > 3 && (package_in.at(4) == CMD_GET_INFO || (package_in.at(4) == CMD_GET_PARAM_LIST && package_in.size() > 7))) { + int special_packet_length = package_in.at(3) << 5; // multiple of 32 + serial_ports_.at(serial_port_name)->read(package_in, special_packet_length - package_in.at(3)); // already read package_in.at(3) bytes + } + } catch (...) { + return -1; + } + return package_in.size(); +} + +int Communication::serializePackage(uint8_t device_id, uint8_t command, std::vector &package_out) { + std::vector data; + return serializePackage(device_id, command, data, package_out); +} + +int Communication::serializePackage(uint8_t device_id, uint8_t command, const std::vector &data, std::vector &package_out) { + package_out.clear(); + package_out.push_back(':'); + package_out.push_back(':'); + package_out.push_back(device_id); + package_out.push_back(2 + data.size()); + package_out.push_back(command); + package_out.insert(std::end(package_out), std::begin(data), std::end(data)); + package_out.push_back(checksum(package_out, package_out.size())); + return package_out.size(); +} + +int Communication::writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command) { + std::vector package_out; + serializePackage(device_id, command, package_out); + return writePackage(serial_port_name, package_out); +} + +int Communication::writePackage(const std::string &serial_port_name, uint8_t device_id, uint8_t command, const std::vector &data) { + std::vector package_out; + serializePackage(device_id, command, data, package_out); + return writePackage(serial_port_name, package_out); +} + +int Communication::writePackage(const std::string &serial_port_name, const std::vector &package_out) { + try { + if (serial_ports_.at(serial_port_name)->write(package_out) != package_out.size()) { + return -1; + } + } catch (...) { + return -1; + } + return package_out.size(); +} + +uint8_t Communication::checksum(const std::vector &data, uint32_t size) { + uint8_t checksum = 0x00; + if (data.size() >= size) { + for (uint32_t i=4; i &data_in) { + data_in.clear(); + uint8_t device_id_in = 0; + uint8_t command_in = 0; + std::vector data_in_buffer; + + // works only for non-broadcast commands + if (!device_id) { + return -1; + } + + while (readPackage(serial_port_name, device_id_in, command_in, data_in_buffer) > 0) { + //WARN: this is ugly because it can lead to unwanted command parsing on CMD_INIT_MEM, CMD_RESTORE_PARAMS or CMD_GET_PARAM_LIST (especially this that is used for many things!) + if (device_id != device_id_in && (command != CMD_INIT_MEM && command != CMD_RESTORE_PARAMS && (command != CMD_GET_PARAM_LIST || data_in_buffer.size() != 1))) { // id and id_in can differ on CMD_INIT_MEM or CMD_RESTORE_PARAMS (and also with CMD_GET_PARAM_LIST when changing the device ID) + continue; + } + //WARN: ACK messages do not carry the command info on legacy devices + if (command_in == command || (command_in == ACK_OK && (command == CMD_INIT_MEM || command == CMD_STORE_PARAMS || command == CMD_STORE_DEFAULT_PARAMS || command == CMD_RESTORE_PARAMS || command == CMD_BOOTLOADER || command == CMD_CALIBRATE || command == CMD_HAND_CALIBRATE))) { + data_in = data_in_buffer; + if (device_id != device_id_in) { + data_in.push_back(device_id_in); // ugly fix to pass the new id to the caller (data_in_buffer is only the ACK response in these cases) + } + return data_in.size(); + } + } + return -1; +} + +int CommunicationLegacy::sendCommandAndParse(const std::string &serial_port_name, uint8_t device_id, uint8_t command, uint8_t max_repeats, const std::vector &data_out, std::vector &data_in) { + uint8_t repeats = 0; + data_in.clear(); + + while (repeats <= max_repeats) { + if (sendCommand(serial_port_name, device_id, command, data_out) < 0) { + repeats++; + continue; + } + + if (command == CMD_STORE_PARAMS || command == CMD_STORE_DEFAULT_PARAMS) { // writing to EEPROM takes a while + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // 100ms should be enough for legacy firmware which write less bytes in EEPROM + } + + // CMD_GET_PARAM_LIST and CMD_GET_INFO packages size exceeds uint8 max value and require a special parsing method + if ((command == CMD_GET_PARAM_LIST && data_out.size() == 2) || command == CMD_GET_INFO) { + std::vector package_in; + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + if (readLongPackage(serial_port_name, package_in) > 0) { + if (command == CMD_GET_PARAM_LIST) { // CMD_GET_INFO is pure ASCII package without packet info (there is no need to trim it) + package_in.erase(package_in.begin(), package_in.begin()+5); // 5 bytes that need to be filtered out (:|:|||) + package_in.erase(package_in.end()-1); // one checksum byte that need to be filtered out + } + data_in = std::vector(std::begin(package_in), std::end(package_in)); + break; + } + } else if (command == CMD_GET_PARAM_LIST) { // set param did non have ACK on legacy devices... + data_in.push_back(1); // fake ACK because legacy devices do not provide ACK on set param + break; + } else { // all other commands + std::vector packages_in; + if (parsePackage(serial_port_name, device_id, command, packages_in) >= 0) { + data_in = packages_in; + break; + } + } + + repeats++; + } + + return repeats <= max_repeats ? data_in.size() : -1; +} + +int CommunicationLegacy::deserializePackage(const std::vector &package_in, uint8_t &device_id, uint8_t &command, std::vector &data) { + if (package_in.size() < 6) { + return -1; + } + if (package_in.at(0) != ':' || package_in.at(1) != ':') { + return -1; + } + if (package_in.at(2) == 0) { + return -1; + } + if (package_in.at(4) != CMD_GET_INFO && (package_in.at(4) != CMD_GET_PARAM_LIST || package_in.size() <= 7)) { // cannot check size for very long packets that exceed 8bit length + if (package_in.at(3) != package_in.size()-4) { + return -1; + } + } + if (package_in.back() != checksum(package_in, package_in.size()-1)) { + return -1; + } + device_id = package_in.at(2); + command = package_in.at(4); + data.clear(); + data.insert(std::end(data), std::begin(package_in)+5, std::end(package_in)-1); + + return package_in.size(); +} + +int CommunicationLegacy::readLongPackage(const std::string &serial_port_name, std::vector &package_in) { + package_in.clear(); + try { + serial_ports_.at(serial_port_name)->read(package_in, 5000); + } catch (...) { + return -1; + } + return package_in.size(); +} + +int CommunicationLegacy::readPackage(const std::string &serial_port_name, uint8_t &device_id, uint8_t &command, std::vector &data) { + std::vector package_in; + if (readPackage(serial_port_name, package_in) < 0) { + return -1; + } + return deserializePackage(package_in, device_id, command, data); +} + +int CommunicationLegacy::readPackage(const std::string &serial_port_name, std::vector &package_in) { + package_in.clear(); + try { + serial_ports_.at(serial_port_name)->read(package_in, 4); + if (package_in.size() != 4) { + serial_ports_.at(serial_port_name)->flush(); + return -1; + } + if (package_in.at(0) != ':' || package_in.at(1) != ':') { + serial_ports_.at(serial_port_name)->flush(); + return -1; + } + serial_ports_.at(serial_port_name)->read(package_in, package_in.at(3)); + } catch (...) { + return -1; + } + return package_in.size(); +} + +Device::Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +Device::Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +Device::Device(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params) + : name_(std::move(name)), + serial_port_(std::move(serial_port)), + params_(std::move(params)), + communication_(std::move(communication)) { + params_->id = id; + if (init_params) { + std::vector param_buffer; + if (getParameters(id, param_buffer) != 0) { + throw std::runtime_error("failure during getParameters()"); + } + params_->initParams(param_buffer); + if (params_->id != id) { + throw std::runtime_error("failure during initParams()"); + } + } +} + +int Device::ping() { + return communication_->sendCommandAndParse(serial_port_, params_->id, CMD_PING); +} + +int Device::getControlReferences(std::vector &control_references) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_INPUTS, data_in) < 0) { + return -1; + } + control_references = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int Device::getCurrents(std::vector ¤ts) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_CURRENTS, data_in) < 0) { + return -1; + } + currents = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int Device::getCurrentsAndPositions(std::vector ¤ts, + std::vector &positions) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_CURR_AND_MEAS, data_in) < 0) { + return -1; + } + auto currents_and_positions = Communication::vectorCastAndSwap(data_in); + currents = std::vector(currents_and_positions.begin(), currents_and_positions.begin()+2); + positions = std::vector(currents_and_positions.begin()+2, currents_and_positions.end()); + return 0; +} + +int Device::getPositions(std::vector &positions) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_MEASUREMENTS, data_in) < 0) { + return -1; + } + positions = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int Device::getVelocities(std::vector &velocities) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_VELOCITIES, data_in) < 0) { + return -1; + } + velocities = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int Device::getAccelerations(std::vector &accelerations) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_ACCEL, data_in) < 0) { + return -1; + } + accelerations = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int Device::getMotorStates(bool &motor_state) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_ACTIVATE, data_in) < 0) { + return -1; + } + if (data_in.size() != 1 || (data_in.front() != 0x03 && data_in.front() != 0x00)) { + return -1; + } + motor_state = data_in.front() == 0x03; + return 0; +} + +int Device::getCycleTime(int16_t &cycle_time) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_CYCLE_TIME, data_in) < 0) { + return -1; + } + cycle_time = Communication::vectorCastAndSwap(data_in).at(1); + return 0; +} + +int Device::setControlReferences(const std::vector &control_references) { + auto const data_out = Communication::vectorSwapAndCast(control_references); + if (communication_->sendCommand(serial_port_, params_->id, CMD_SET_INPUTS, data_out) < 0) { + return -1; + } + return 0; +} + +int Device::setControlReferencesAndWait(const std::vector &control_references) { + auto const data_out = Communication::vectorSwapAndCast(control_references); + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_SET_INPUTS_ACK, data_out, data_in) < 0) { + return -1; + } + if (data_in.size() < 0) { + return data_in.size(); + } + return 0; +} + +int Device::setMotorStates(bool motor_state) { + uint8_t data_out = motor_state ? 0x03 : 0x00; + if (communication_->sendCommand(serial_port_, params_->id, CMD_ACTIVATE, std::vector(1, data_out)) < 0) { + return -1; + } + return 0; +} + +int Device::getInfo(std::string &info) { + return getInfo(0, info); +} + +int Device::getInfo(uint16_t info_type, std::string &info) { + std::vector data_in; + const std::vector data_out = Communication::vectorSwapAndCast({info_type}); + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_INFO, data_out, data_in) < 0) { + return -1; + } + info.assign(data_in.begin(), data_in.end()); + return 0; +} + +int Device::getParameters(std::vector ¶m_buffer) { + return getParameters(params_->id, param_buffer); +} + +int Device::getParameters(uint8_t id, std::vector ¶m_buffer) { + param_buffer.clear(); + const std::vector data_out = Communication::vectorSwapAndCast({0x0000}); + if (communication_->sendCommandAndParse(serial_port_, id, CMD_GET_PARAM_LIST, data_out, param_buffer) < 0) { + return -1; + } + return 0; +} + +int Device::getParamId() { + return getParamId(params_->id); +} + +int Device::getParamId(uint8_t &id) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(1, param_buffer, id); + return 0; +} + +int Device::getParamPositionPID() { + return getParamPositionPID(params_->position_pid); +} + +int Device::getParamPositionPID(std::vector &position_pid) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(2, param_buffer, position_pid); + return 0; +} + +int Device::getParamCurrentPID() { + return getParamCurrentPID(params_->current_pid); +} + +int Device::getParamCurrentPID(std::vector ¤t_pid) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(3, param_buffer, current_pid); + return 0; +} + +int Device::getParamStartupActivation() { + return getParamStartupActivation(params_->startup_activation); +} + +int Device::getParamStartupActivation(uint8_t &startup_activation) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(4, param_buffer, startup_activation); + return 0; +} + +int Device::getParamInputMode() { + return getParamInputMode(params_->input_mode); +} + +int Device::getParamInputMode(uint8_t &input_mode) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(5, param_buffer, input_mode); + return 0; +} + +int Device::getParamControlMode() { + return getParamControlMode(params_->control_mode); +} + +int Device::getParamControlMode(uint8_t &control_mode) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(6, param_buffer, control_mode); + return 0; +} + +int Device::getParamEncoderResolutions() { + return getParamEncoderResolutions(params_->encoder_resolutions); +} + +int Device::getParamEncoderResolutions(std::vector &encoder_resolutions) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(7, param_buffer, encoder_resolutions); + return 0; +} + +int Device::getParamEncoderOffsets() { + return getParamEncoderOffsets(params_->encoder_offsets); +} + +int Device::getParamEncoderOffsets(std::vector &encoder_offsets) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(8, param_buffer, encoder_offsets); + return 0; +} + +int Device::getParamEncoderMultipliers() { + return getParamEncoderMultipliers(params_->encoder_multipliers); +} + +int Device::getParamEncoderMultipliers(std::vector &encoder_multipliers) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(9, param_buffer, encoder_multipliers); + return 0; +} + +int Device::getParamUsePositionLimits() { + return getParamUsePositionLimits(params_->use_position_limits); +} + +int Device::getParamUsePositionLimits(uint8_t &use_position_limits) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(10, param_buffer, use_position_limits); + return 0; +} + +int Device::getParamPositionLimits() { + return getParamPositionLimits(params_->position_limits); +} + +int Device::getParamPositionLimits(std::vector &position_limits) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(11, param_buffer, position_limits); + return 0; +} + +int Device::getParamPositionMaxSteps() { + return getParamPositionMaxSteps(params_->position_max_steps); +} + +int Device::getParamPositionMaxSteps(std::vector &position_max_steps) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(12, param_buffer, position_max_steps); + return 0; +} + +int Device::getParamCurrentLimit() { + return getParamCurrentLimit(params_->current_limit); +} + +int Device::getParamCurrentLimit(int16_t ¤t_limit) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(13, param_buffer, current_limit); + return 0; +} + +int Device::setParameter(uint16_t param_type, const std::vector ¶m_data) { + bool motor_state = true; + if (getMotorStates(motor_state) || motor_state) { + return -1; // motor must be stopped while changing parameters + } + + std::vector data_in; + std::vector param_payload = Communication::vectorSwapAndCast({param_type}); + param_payload.insert(param_payload.end(), param_data.begin(), param_data.end()); + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_PARAM_LIST, param_payload, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + return 0; +} + +int Device::setParamId(uint8_t id) { + int set_fail = setParameter(1, Communication::vectorSwapAndCast({id})); + if (!set_fail) { + params_->id = id; + } + return set_fail; +} + +int Device::setParamPositionPID(const std::vector &position_pid) { + int set_fail = setParameter(2, Communication::vectorSwapAndCast(position_pid)); + if (!set_fail) { + params_->position_pid = position_pid; + } + return set_fail; +} + +int Device::setParamCurrentPID(const std::vector ¤t_pid) { + int set_fail = setParameter(3, Communication::vectorSwapAndCast(current_pid)); + if (!set_fail) { + params_->current_pid = current_pid; + } + return set_fail; +} + +int Device::setParamStartupActivation(bool startup_activation) { + int set_fail = setParameter(4, Communication::vectorSwapAndCast({startup_activation})); + if (!set_fail) { + params_->startup_activation = startup_activation; + } + return set_fail; +} + +int Device::setParamInputMode(uint8_t input_mode) { + int set_fail = setParameter(5, Communication::vectorSwapAndCast({input_mode})); + if (!set_fail) { + params_->input_mode = input_mode; + } + return set_fail; +} + +int Device::setParamControlMode(uint8_t control_mode) { + int set_fail = setParameter(6, Communication::vectorSwapAndCast({control_mode})); + if (!set_fail) { + params_->control_mode = control_mode; + } + return set_fail; +} + +int Device::setParamEncoderResolutions(const std::vector &encoder_resolutions) { + int set_fail = setParameter(7, Communication::vectorSwapAndCast(encoder_resolutions)); + if (!set_fail) { + params_->encoder_resolutions = encoder_resolutions; + } + return set_fail; +} + +int Device::setParamEncoderOffsets(const std::vector &encoder_offsets) { + int set_fail = setParameter(8, Communication::vectorSwapAndCast(encoder_offsets)); + if (!set_fail) { + params_->encoder_offsets = encoder_offsets; + } + return set_fail; +} + +int Device::setParamEncoderMultipliers(const std::vector &encoder_multipliers) { + int set_fail = setParameter(9, Communication::vectorSwapAndCast(encoder_multipliers)); + if (!set_fail) { + params_->encoder_multipliers = encoder_multipliers; + } + return set_fail; +} + +int Device::setParamUsePositionLimits(bool use_position_limits) { + int set_fail = setParameter(10, Communication::vectorSwapAndCast({use_position_limits})); + if (!set_fail) { + params_->use_position_limits = use_position_limits; + } + return set_fail; +} + +int Device::setParamPositionLimits(const std::vector &position_limits) { + int set_fail = setParameter(11, Communication::vectorSwapAndCast(position_limits)); + if (!set_fail) { + params_->position_limits = position_limits; + } + return set_fail; +} + +int Device::setParamPositionMaxSteps(const std::vector &position_max_steps) { + int set_fail = setParameter(12, Communication::vectorSwapAndCast(position_max_steps)); + if (!set_fail) { + params_->position_max_steps = position_max_steps; + } + return set_fail; +} + +int Device::setParamCurrentLimit(int16_t current_limit) { + int set_fail = setParameter(13, Communication::vectorSwapAndCast({current_limit})); + if (!set_fail) { + params_->current_limit = current_limit; + } + return set_fail; +} + +int Device::setParamZeros() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_SET_ZEROS, data_in) < 0) { + return -1; + } + return getParamEncoderOffsets(); // update intenal params_->encoder_offsets values +} + +int Device::setParamBaudrate(uint8_t prescaler_divider) { + const std::vector data_out = Communication::vectorSwapAndCast({prescaler_divider}); + if (communication_->sendCommand(serial_port_, params_->id, CMD_SET_BAUDRATE, data_out) < 0) { + return -1; + } + return 0; +} + +void Device::Params::initParams(const std::vector ¶m_buffer) { + //FIXME: hardcoded input mode parameter id + getParameter(1, param_buffer, id); + getParameter(2, param_buffer, position_pid); + getParameter(3, param_buffer, current_pid); + getParameter(4, param_buffer, startup_activation); + getParameter(5, param_buffer, input_mode); + getParameter(6, param_buffer, control_mode); + getParameter(7, param_buffer, encoder_resolutions); + getParameter(8, param_buffer, encoder_offsets); + getParameter(9, param_buffer, encoder_multipliers); + getParameter(10, param_buffer, use_position_limits); + getParameter(11, param_buffer, position_limits); + getParameter(12, param_buffer, position_max_steps); + getParameter(13, param_buffer, current_limit); +} + +int Device::restoreFactoryDataMemory() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_INIT_MEM, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + if (data_in.size() > 0) { // if data_in is empty the ID is the same + params_->id = data_in.back(); // the package contains the ACK + the new device id + } + return 0; +} + +int Device::restoreUserDataMemory() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_RESTORE_PARAMS, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + if (data_in.size() > 0) { // if data_in is empty the ID is the same + params_->id = data_in.back(); // the package contains the ACK + the new device id + } + return 0; +} + +int Device::storeUserDataMemory() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_STORE_PARAMS, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + return 0; +} + +int Device::storeFactoryDataMemory() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_STORE_DEFAULT_PARAMS, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + return 0; +} + +int Device::setBootloaderMode() { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_BOOTLOADER, data_in) < 0) { + return -1; + } + if (data_in.size() > 0 && data_in.front() != 1) { // check ACK + return -2; + } + return 0; +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api_wrapper.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api_wrapper.cpp new file mode 100644 index 0000000..4428407 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbrobotics_research_api_wrapper.cpp @@ -0,0 +1,464 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +std::shared_ptr handler_g = nullptr; + +void setFakeValidHandle(comm_settings *handle) { +#ifndef _WIN32 + handle->file_handle = 1; +#else + handle->file_handle = nullptr; +#endif +} + +void setInvalidHandle(comm_settings *handle) { +#ifndef _WIN32 + handle->file_handle = -1; +#else + handle->file_handle = INVALID_HANDLE_VALUE; +#endif +} + +void openRS485(comm_settings *handle, const char *serial_port_name, int /*BAUD_RATE*/) { + std::cerr << "\n\n\n" + " ------------------------------------------------------------------------------\n" + " WARNING: This API wrapper is now deprecated!\n" + " ------------------------------------------------------------------------------\n" + " * All the methods of the qbrobotics C++ API v6.2.x are still usable but they\n" + " are now deprecated and will be no longer supported in following releases.\n" + " * Please update your code to the new C++ API v7.x.x which adds also many\n" + " improvements in efficiency and reliability.\n" + " * See the documentation at https://bitbucket.org/qbrobotics/qbrobotics-driver\n" + " for more details about the new API, and contact support@qbrobotics.com for\n" + " any questions or concerns.\n" + " ------------------------------------------------------------------------------\n\n" << std::endl; + + char serial_ports[60][255] = {}; + bool serial_port_found = false; + int number_of_ports = RS485listPorts(serial_ports); + for (int i=0; iopenSerialPort(std::string(serial_port_name))) { + handle->serial_port_name = std::string(serial_port_name); + setFakeValidHandle(handle); // fake positive handle + return; + } + } + } + + if (!serial_port_found) { // try to open it anyway + if (!handler_g->createSerialPort(std::string(serial_port_name))) { + handle->serial_port_name = std::string(serial_port_name); + setFakeValidHandle(handle); // fake positive handle + return; + } + } + + handle->serial_port_name.clear(); + setInvalidHandle(handle); +} + +void closeRS485(comm_settings *handle) { + if (!handle->serial_port_name.empty()) { + handler_g->closeSerialPort(handle->serial_port_name); + handle->serial_port_name.clear(); + setInvalidHandle(handle); + } +} + +void RS485GetInfo(comm_settings *handle, char *buffer) { + std::cerr << "\n\n\n" + " ------------------------------------------------------------------------------\n" + " WARNING: RS485GetInfo() is not safe!\n" + " ------------------------------------------------------------------------------\n" + " * This method is exactly the same as sending commGetInfo() in broadcast mode,\n" + " i.e. by setting the ID equals 0.\n" + " * If there is only one device connected on the given serial port, this method\n" + " is safe as it is the commGetInfo(). However, when there are several devices\n" + " in chain, it is not possible to distinguish the received packages from one\n" + " to the other, and the behavior is therefore unpredictable.\n" + " * Please use the similar commGetInfo() method instead and if needed list the\n" + " connected device IDs with RS485ListDevices() before the call.\n" + " ------------------------------------------------------------------------------\n\n" << std::endl; +} + +int RS485ListDevices(comm_settings *handle, char list_of_ids[255]) { + if (!handle->serial_port_name.empty()) { + std::vector device_ids; + if (handler_g->listConnectedDevices(handle->serial_port_name, device_ids) >= 0) { + for (size_t i=0; i serial_ports; + handler_g = std::make_shared(); + if (handler_g->listSerialPorts(serial_ports) < 0 || serial_ports.size() > 60) { + return -1; + } + for (size_t i=0; iserial_port_name.empty()) { + uint8_t device_id = 0; + uint8_t command = 0; + std::vector buffer; + if (handler_g->readPackage(handle->serial_port_name, device_id, command, buffer) > 0 && device_id == id) { + std::memcpy(package, buffer.data(), buffer.size()); + return buffer.size(); + } + } + return -1; +} + +void commActivate(comm_settings *handle, int id, char activate) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + dev.setMotorStates(activate); + } +} + +int commBootloader(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.setBootloaderMode(); + } + return -1; +} + +int commCalibrate(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::qbmoveResearch qbmove(handler_g, "dev", handle->serial_port_name, id, false); + return qbmove.computeAndStoreMaximumStiffness(); + } + return -1; +} + +int commGetAccelerations(comm_settings *handle, int id, short int *accelerations) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector accelerations_in; + if (dev.getAccelerations(accelerations_in)) { + return -1; + } + accelerations[0] = accelerations_in.at(0); + accelerations[1] = accelerations_in.at(1); + accelerations[2] = accelerations_in.at(2); + return 0; + } + return -1; +} + +int commGetActivate(comm_settings *handle, int id, char *activate) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + bool motor_state_in = false; + if (dev.getMotorStates(motor_state_in)) { + return -1; + } + *activate = motor_state_in ? 0x03 : 0x00; + return 0; + } + return -1; +} + +int commGetCurrents(comm_settings *handle, int id, short int *currents) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector currents_in; + if (dev.getCurrents(currents_in)) { + return -1; + } + currents[0] = currents_in.at(0); + currents[1] = currents_in.at(1); + return 0; + } + return -1; +} + +int commGetCurrAndMeas(comm_settings *handle, int id, short int *values) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector currents_in; + std::vector positions_in; + if (dev.getCurrentsAndPositions(currents_in, positions_in)) { + return -1; + } + values[0] = currents_in.at(0); + values[1] = currents_in.at(1); + values[2] = positions_in.at(0); + values[3] = positions_in.at(1); + values[4] = positions_in.at(2); + return 0; + } + return -1; +} + +int commGetEmg(comm_settings *handle, int id, short int *emgs) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::qbSoftHandResearch qbsofthand(handler_g, "dev", handle->serial_port_name, id, false); + std::vector emg_values_in; + if (qbsofthand.getEMGs(emg_values_in)) { + return -1; + } + emgs[0] = emg_values_in.at(0); + emgs[1] = emg_values_in.at(1); + return 0; + } + return -1; +} + +int commGetInfo(comm_settings *handle, int id, short int info_type, char *info) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::string info_in; + if (dev.getInfo(info_type, info_in)) { + return -1; + } + std::strcpy(info, info_in.c_str()); + return 0; + } + return -1; +} + +int commGetInputs(comm_settings *handle, int id, short int *inputs) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector references_in; + if (dev.getControlReferences(references_in)) { + return -1; + } + inputs[0] = references_in.at(0); + inputs[1] = references_in.at(1); + return 0; + } + return -1; +} + +int commGetJoystick(comm_settings *handle, int id, short int *joystick) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::qbSoftHandResearch qbsofthand(handler_g, "dev", handle->serial_port_name, id, false); + std::vector joystick_values_in; + if (qbsofthand.getJoystick(joystick_values_in)) { + return -1; + } + joystick[0] = joystick_values_in.at(0); + joystick[1] = joystick_values_in.at(1); + return 0; + } + return -1; +} + +int commGetMeasurements(comm_settings *handle, int id, short int *positions) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector positions_in; + if (dev.getPositions(positions_in)) { + return -1; + } + positions[0] = positions_in.at(0); + positions[1] = positions_in.at(1); + positions[2] = positions_in.at(2); + return 0; + } + return -1; +} + +int commGetParamList(comm_settings *handle, int id, unsigned short index, void *values, unsigned short value_size, unsigned short num_of_values, uint8_t *buffer) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + if(!index) { + std::vector buffer_in; + dev.getParameters(buffer_in); + std::memcpy(buffer, buffer_in.data(), buffer_in.size()); + return 0; + } else { + std::vector data_out; + switch (value_size) { + case 1: { + std::vector vector({reinterpret_cast(values), reinterpret_cast(values)+num_of_values}); + data_out = qbrobotics_research_api::Communication::vectorSwapAndCast(vector); + } break; + case 2: { + std::vector vector({reinterpret_cast(values), reinterpret_cast(values)+num_of_values}); + data_out = qbrobotics_research_api::Communication::vectorSwapAndCast(vector); + } break; + case 4: { + std::vector vector({reinterpret_cast(values), reinterpret_cast(values)+num_of_values}); + data_out = qbrobotics_research_api::Communication::vectorSwapAndCast(vector); + } break; + default: + return -1; + } + return dev.setParameter(index, data_out); + } + } + return -1; +} + +int commGetVelocities(comm_settings *handle, int id, short int *velocities) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + std::vector velocities_in; + if (dev.getVelocities(velocities_in)) { + return -1; + } + velocities[0] = velocities_in.at(0); + velocities[1] = velocities_in.at(1); + velocities[2] = velocities_in.at(2); + return 0; + } + return -1; +} + +int commHandCalibrate(comm_settings *handle, int id, short int speed, short int repetitions) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::qbSoftHandResearch qbsofthand(handler_g, "dev", handle->serial_port_name, id, false); + return qbsofthand.openCloseCycles(speed, repetitions); + } + return -1; +} + +int commInitMem(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.restoreFactoryDataMemory(); + } + return -1; +} + +int commPing(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.ping(); + } + return -1; +} + +int commRestoreParams(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.restoreUserDataMemory(); + } + return -1; +} + +void commSetBaudRate(comm_settings *handle, int id, short int baudrate) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + dev.setParamBaudrate(!baudrate ? 3 : 13); + } +} + +void commSetCuffInputs(comm_settings *handle, int id, int flag) { + //TODO: this method should be ported in the new API under Cuff device class + if (!handle->serial_port_name.empty()) { + auto const data_out = std::vector(flag ? 1 : 0); + handler_g->sendCommand(handle->serial_port_name, id, CMD_SET_CUFF_INPUTS, data_out); + } +} + +void commSetInputs(comm_settings *handle, int id, short int *inputs) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + dev.setControlReferences({inputs[0], inputs[1]}); + } +} + +int commSetInputsAck(comm_settings *handle, int id, short int *inputs) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + dev.setControlReferences({inputs[0], inputs[1]}); + return 0; + } else { + return -1; + } +} + +void commSetPosStiff(comm_settings *handle, int id, short int *inputs) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::qbmoveResearch qbmove(handler_g, "dev", handle->serial_port_name, id, false); + qbmove.setPositionAndStiffnessReferences(inputs[0], inputs[1]); + } +} + +void commSetWatchDog(comm_settings *handle, int id, short int wdt) { + std::cerr << "\n\n\n" + " ------------------------------------------------------------------------------\n" + " WARNING: commSetWatchDog() is already deprecated!\n" + " ------------------------------------------------------------------------------\n" + " * This method has not been ported to the new API and therefore it will be no\n" + " longer supported at all in future releases.\n" + " * Please contact support@qbrobotics.com for any questions, concerns or feature\n" + " requests.\n" + " ------------------------------------------------------------------------------\n\n" << std::endl; + + if (!handle->serial_port_name.empty()) { + auto const data_out = std::vector({static_cast(wdt/2)}); + handler_g->sendCommand(handle->serial_port_name, id, CMD_SET_WATCHDOG, data_out); + } +} + +int commSetZeros(comm_settings *handle, int id, void *values, unsigned short num_of_values) { + std::cerr << "\n\n\n" + " ------------------------------------------------------------------------------\n" + " WARNING: commSetZeros() differs from v6.2.x!\n" + " ------------------------------------------------------------------------------\n" + " * This method uses now an internal computation and does no longer use `values`" + " and `num_of_values`.\n" + " * Please contact support@qbrobotics.com for any questions, concerns or feature\n" + " requests.\n" + " ------------------------------------------------------------------------------\n\n" << std::endl; + + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.setParamZeros(); + } + return -1; +} + +int commStoreParams(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.storeUserDataMemory(); + } + return -1; +} + +int commStoreDefaultParams(comm_settings *handle, int id) { + if (!handle->serial_port_name.empty()) { + qbrobotics_research_api::Device dev(handler_g, "dev", handle->serial_port_name, id, false); + return dev.storeFactoryDataMemory(); + } + return -1; +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand2_research_api.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand2_research_api.cpp new file mode 100644 index 0000000..890bb47 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand2_research_api.cpp @@ -0,0 +1,184 @@ +#include +#include + +using namespace qbrobotics_research_api; + +void qbSoftHand2MotorsResearch::Params::initParams(const std::vector ¶m_buffer) { + Device::Params::initParams(param_buffer); + // Legacy firmware needs inverted signs for second motor encoders (encoder 3 and encoder 4) + std::for_each(encoder_offsets.begin()+2, encoder_offsets.end(), [](int16_t &x){ x *= -1; } ); + + getParameter(25, param_buffer, hand_side); +} + +qbSoftHand2MotorsResearch::qbSoftHand2MotorsResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +qbSoftHand2MotorsResearch::qbSoftHand2MotorsResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +/*------ Methods for 6.X.X firmware ------*/ +int qbSoftHand2MotorsResearch::setMotorStates(bool motor_state) { + int set_fail = Device::setMotorStates(motor_state); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + return set_fail; +} + +int qbSoftHand2MotorsResearch::setParameter(uint16_t param_type, const std::vector ¶m_data) { + int set_fail = Device::setParameter(param_type, param_data); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + if (!set_fail && param_type != 1) { // WARN: the storeUserDataMemory for ID change is called by the specific method + set_fail = Device::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + return set_fail; +} + +int qbSoftHand2MotorsResearch::setParamHandSide(uint8_t hand_side) { + int set_fail = setParameter(25, Communication::vectorSwapAndCast({hand_side})); + return set_fail; +} + +int qbSoftHand2MotorsResearch::setParamId(uint8_t id) { // old method only for legacy devices + uint8_t previous_id = params_->id; + int set_fail = Device::setParamId(id); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + if (!set_fail) { + params_->id = previous_id; // WARN: need to send the command to the previous ID on legacy devices! + set_fail = Device::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + params_->id = id; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + return set_fail; +} + +int qbSoftHand2MotorsResearch::setParamZeros() { // old method only for legacy devices + std::vector positions; + if (getPositions(positions)) { + return -1; + } + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + std::vector offsets; + if (getParamEncoderOffsets(offsets)) { + return -1; + } + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + std::transform(offsets.begin(), offsets.end(), positions.begin(), offsets.begin(), std::minus()); + return setParamEncoderOffsets(offsets); +} + +/*------ Methods for SoftHand 2 ------*/ + +int qbSoftHand2MotorsResearch::getParamEncoderOffsets() { + return getParamEncoderOffsets(params_->encoder_offsets); +} + +int qbSoftHand2MotorsResearch::getParamEncoderOffsets(std::vector &encoder_offsets) { + int set_fail = Device::getParamEncoderOffsets(encoder_offsets); + // Legacy firmware needs inverted signs for second motor encoders (encoder 3 and encoder 4) + std::for_each( encoder_offsets.begin()+2, encoder_offsets.end(), [](int16_t &x){x *= -1;} ); + return set_fail; +} + +int qbSoftHand2MotorsResearch::getParamHandSide() { + return getParamHandSide(std::dynamic_pointer_cast(params_)->hand_side); +} + +int qbSoftHand2MotorsResearch::getParamHandSide(uint8_t &hand_side) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(25, param_buffer, hand_side); + return 0; +} + +int qbSoftHand2MotorsResearch::getSynergies(std::vector &synergies) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_SYNERGIES, data_in) < 0) { + return -1; + } + synergies = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int qbSoftHand2MotorsResearch::setParamEncoderOffsets(const std::vector &encoder_offsets) { + std::vector legacy_offsets = encoder_offsets; + // Legacy firmware needs inverted signs for second motor encoders (encoder 3 and encoder 4) + std::for_each( legacy_offsets.begin()+2, legacy_offsets.end(), [](int16_t &x){ x *= -1; } ); + return Device::setParamEncoderOffsets(legacy_offsets); +} + +int qbSoftHand2MotorsResearch::setAdditiveSynergiesReferences(int16_t synergy_1, int16_t synergy_2) { + auto const data_out = Communication::vectorSwapAndCast({synergy_1, synergy_2}); + if (communication_->sendCommand(serial_port_, params_->id, CMD_SET_SYNERGIES, data_out) < 0) { + return -1; + } + return 0; +} + +int qbSoftHand2MotorsResearch::setMultiplicativeSynergiesReferences(float synergy_1, float synergy_2) { + if(synergy_1 < 0 || synergy_1 > 1 || synergy_2 < -1 || synergy_2 > 1) { + return -2; + } + int32_t l = getParams()->position_limits[0]; + int32_t L = getParams()->position_limits[1]; + int32_t Lr = (L + l); + float Sf = (2/(float)Lr); + float mf = (1/(float)l); + // multiplicative synergies formulas + int16_t q1 = (1 + synergy_2)*synergy_1/Sf - synergy_2/mf; + int16_t q2 = (1 - synergy_2)*synergy_1/Sf + synergy_2/mf; + + std::vector refs{q1, q2}; + + setControlReferences(refs); +} + +int qbSoftHand2MotorsResearch::setHomePosition() { + // Get the params to return to home position + std::vector synergies; + int result = getSynergies(synergies); + std::vector limits; + getParamPositionLimits(limits); + + if(synergies.size() <= 0 || result < 0 || limits.size() < 4){ // something went wrong + return result; + } + std::vector> synergies_references{{limits.at(0), 0}, {synergies.at(0), 0}}; //|synergies references to cancel synergy 1 | synergies references to cancel synergy 2| + std::vector synergies_to_reach {-1000, 200}; + + int synergy_to_cancel = 1; + while(!synergies_references.empty()) { + if(setAdditiveSynergiesReferences(synergies_references.back().at(0), synergies_references.back().at(1)) != 0){ // cancel the contribution of synergies + return -1; + } + auto t_start = std::chrono::high_resolution_clock::now(); + auto t_end = t_start; + double elapsed_time_ms; + bool condition = true; + while(condition){ + getSynergies(synergies); + if(synergy_to_cancel == 1) { + condition = std::abs(synergies.at(synergy_to_cancel)) > synergies_to_reach.back(); + } else { + condition = synergies.at(synergy_to_cancel) > synergies_to_reach.back(); + } + //condition = synergy_to_cancel == 1 ? std::abs(synergies.at(synergy_to_cancel)) > synergies_to_reach.back() : synergies.at(synergy_to_cancel) > synergies_to_reach.back(); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); //sleep 1 ms in order to not clog the communication with the device + t_end = std::chrono::high_resolution_clock::now(); + elapsed_time_ms = std::chrono::duration(t_end-t_start).count(); + if(elapsed_time_ms > 2000.0) { + result = -2; + break; + } + } + synergies_references.pop_back(); + synergies_to_reach.pop_back(); + synergy_to_cancel --; + } + if(setControlReferences({0, 0}) != 0){ + return -1; + } + return result; +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand_research_api.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand_research_api.cpp new file mode 100644 index 0000000..dc95520 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/libs/research/src/qbsofthand_research_api.cpp @@ -0,0 +1,319 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2021, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include + +using namespace qbrobotics_research_api; + +void qbSoftHandResearch::Params::initParams(const std::vector ¶m_buffer) { + Device::Params::initParams(param_buffer); + getParameter(14, param_buffer, use_emg_calibration); + getParameter(15, param_buffer, emg_thresholds); + getParameter(16, param_buffer, emg_max_value); + getParameter(17, param_buffer, emg_speed); + getParameter(18, param_buffer, use_double_encoder); + getParameter(19, param_buffer, handle_ratio); + getParameter(20, param_buffer, use_pwm_rescaling); + getParameter(21, param_buffer, current_lookup_table); + getParameter(22, param_buffer, rate_limiter); +} + +qbSoftHandResearch::qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +qbSoftHandResearch::qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +qbSoftHandResearch::qbSoftHandResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params) + : Device(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::move(params)) {} + +int qbSoftHandResearch::openCloseCycles(uint16_t speed, uint16_t cycles) { + std::vector data_in; + const std::vector data_out = Communication::vectorSwapAndCast({speed, cycles}); + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_HAND_CALIBRATE, data_out, data_in) < 0) { + return -1; + } + return 0; +} + +int qbSoftHandResearch::getEMGs(std::vector &emg_values) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_EMG, data_in) < 0) { + return -1; + } + emg_values = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int qbSoftHandResearch::getJoystick(std::vector &joystick_values) { + std::vector data_in; + if (communication_->sendCommandAndParse(serial_port_, params_->id, CMD_GET_JOYSTICK, data_in) < 0) { + return -1; + } + joystick_values = Communication::vectorCastAndSwap(data_in); + return 0; +} + +int qbSoftHandResearch::getParamUseEMGCalibration() { + return getParamUseEMGCalibration(std::dynamic_pointer_cast(params_)->use_emg_calibration); +} + +int qbSoftHandResearch::getParamUseEMGCalibration(uint8_t &use_emg_calibration) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(14, param_buffer, use_emg_calibration); + return 0; +} + +int qbSoftHandResearch::getParamEMGThresholds() { + return getParamEMGThresholds(std::dynamic_pointer_cast(params_)->emg_thresholds); +} + +int qbSoftHandResearch::getParamEMGThresholds(std::vector &emg_thresholds) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(15, param_buffer, emg_thresholds); + return 0; +} + +int qbSoftHandResearch::getParamEMGMaxValues() { + return getParamEMGMaxValues(std::dynamic_pointer_cast(params_)->emg_max_value); +} + +int qbSoftHandResearch::getParamEMGMaxValues(std::vector &emg_max_value) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(16, param_buffer, emg_max_value); + return 0; +} + +int qbSoftHandResearch::getParamEMGSpeed() { + return getParamEMGSpeed(std::dynamic_pointer_cast(params_)->emg_speed); +} + +int qbSoftHandResearch::getParamEMGSpeed(uint8_t &emg_speed) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(17, param_buffer, emg_speed); + return 0; +} + +int qbSoftHandResearch::getParamUseDoubleEncoder() { + return getParamUseDoubleEncoder(std::dynamic_pointer_cast(params_)->use_double_encoder); +} + +int qbSoftHandResearch::getParamUseDoubleEncoder(uint8_t &use_double_encoder) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(18, param_buffer, use_double_encoder); + return 0; +} + +int qbSoftHandResearch::getParamHandleRatio() { + return getParamHandleRatio(std::dynamic_pointer_cast(params_)->handle_ratio); +} + +int qbSoftHandResearch::getParamHandleRatio(int8_t &handle_ratio) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(19, param_buffer, handle_ratio); + return 0; +} + +int qbSoftHandResearch::getParamUsePWMRescaling() { + return getParamUsePWMRescaling(std::dynamic_pointer_cast(params_)->use_pwm_rescaling); +} + +int qbSoftHandResearch::getParamUsePWMRescaling(uint8_t &use_pwm_rescaling) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(20, param_buffer, use_pwm_rescaling); + return 0; +} + +int qbSoftHandResearch::getParamCurrentLookupTable() { + return getParamCurrentLookupTable(std::dynamic_pointer_cast(params_)->current_lookup_table); +} + +int qbSoftHandResearch::getParamCurrentLookupTable(std::vector ¤t_lookup_table) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(21, param_buffer, current_lookup_table); + return 0; +} + +int qbSoftHandResearch::getParamRateLimiter() { + return getParamRateLimiter(params_->rate_limiter); +} + +int qbSoftHandResearch::getParamRateLimiter(uint8_t &rate_limiter) { + std::vector param_buffer; + if (getParameters(param_buffer) != 0) { + return -1; + } + Params::getParameter(22, param_buffer, rate_limiter); + return 0; +} + +int qbSoftHandResearch::setParamUseEMGCalibration(uint8_t use_emg_calibration) { + int set_fail = setParameter(14, Communication::vectorSwapAndCast({use_emg_calibration})); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->use_emg_calibration = use_emg_calibration; + } + return set_fail; +} + +int qbSoftHandResearch::setParamEMGThresholds(const std::vector &emg_thresholds) { + int set_fail = setParameter(15, Communication::vectorSwapAndCast(emg_thresholds)); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->emg_thresholds = emg_thresholds; + } + return set_fail; +} + +int qbSoftHandResearch::setParamEMGMaxValues(const std::vector &emg_max_value) { + int set_fail = setParameter(16, Communication::vectorSwapAndCast(emg_max_value)); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->emg_max_value = emg_max_value; + } + return set_fail; +} + +int qbSoftHandResearch::setParamEMGSpeed(uint8_t emg_speed) { + int set_fail = setParameter(17, Communication::vectorSwapAndCast({emg_speed})); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->emg_speed = emg_speed; + } + return set_fail; +} + +int qbSoftHandResearch::setParamUseDoubleEncoder(uint8_t use_double_encoder) { + int set_fail = setParameter(18, Communication::vectorSwapAndCast({use_double_encoder})); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->use_double_encoder = use_double_encoder; + } + return set_fail; +} + +int qbSoftHandResearch::setParamHandleRatio(int8_t handle_ratio) { + int set_fail = setParameter(19, Communication::vectorSwapAndCast({handle_ratio})); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->handle_ratio = handle_ratio; + } + return set_fail; +} + +int qbSoftHandResearch::setParamUsePWMRescaling(uint8_t use_pwm_rescaling) { + int set_fail = setParameter(20, Communication::vectorSwapAndCast({use_pwm_rescaling})); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->use_pwm_rescaling = use_pwm_rescaling; + } + return set_fail; +} + +int qbSoftHandResearch::setParamCurrentLookupTable(const std::vector ¤t_lookup_table) { + int set_fail = setParameter(21, Communication::vectorSwapAndCast(current_lookup_table)); + if (!set_fail) { + std::dynamic_pointer_cast(params_)->current_lookup_table = current_lookup_table; + } + return set_fail; +} + +int qbSoftHandResearch::setParamRateLimiter(uint8_t rate_limiter) { + int set_fail = setParameter(22, Communication::vectorSwapAndCast({rate_limiter})); + if (!set_fail) { + params_->rate_limiter = rate_limiter; + } + return set_fail; +} + + +// ---------------------------------------------------------------- + + +qbSoftHandLegacyResearch::qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id) + : qbSoftHandResearch(std::move(communication), std::move(name), std::move(serial_port), id, true, std::make_unique()) {} + +qbSoftHandLegacyResearch::qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params) + : qbSoftHandResearch(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::make_unique()) {} + +qbSoftHandLegacyResearch::qbSoftHandLegacyResearch(std::shared_ptr communication, std::string name, std::string serial_port, uint8_t id, bool init_params, std::unique_ptr params) + : qbSoftHandResearch(std::move(communication), std::move(name), std::move(serial_port), id, init_params, std::move(params)) {} + +int qbSoftHandLegacyResearch::setMotorStates(bool motor_state) { + int set_fail = qbSoftHandResearch::setMotorStates(motor_state); + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + return set_fail; +} + +int qbSoftHandLegacyResearch::setParameter(uint16_t param_type, const std::vector ¶m_data) { + int set_fail = qbSoftHandResearch::setParameter(param_type, param_data); + if (!set_fail && param_type != 1) { // WARN: the storeUserDataMemory for ID change is called by the specific method + set_fail = qbSoftHandResearch::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + } + return set_fail; +} + +int qbSoftHandLegacyResearch::setParamId(uint8_t id) { // old method only for legacy devices + uint8_t previous_id = params_->id; + int set_fail = qbSoftHandResearch::setParamId(id); + if (!set_fail) { + params_->id = previous_id; // WARN: need to send the command to the previous ID on legacy devices! + set_fail = qbSoftHandResearch::storeUserDataMemory(); // WARN: a store user memory is required on legacy devices! + params_->id = id; + } + return set_fail; +} + +int qbSoftHandLegacyResearch::setParamZeros() { // old method only for legacy devices + std::vector positions; + if (getPositions(positions)) { + return -1; + } + std::vector offsets; + if (getParamEncoderOffsets(offsets)) { + return -1; + } + std::transform(offsets.begin(), offsets.end(), positions.begin(), offsets.begin(), std::minus()); + return setParamEncoderOffsets(offsets); +} \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/qbrobotics_driverConfig.cmake.in b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/qbrobotics_driverConfig.cmake.in new file mode 100644 index 0000000..369d80b --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/qbrobotics_driverConfig.cmake.in @@ -0,0 +1,9 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) +find_dependency(Serial) + +set_and_check(qbrobotics_driver_TARGETS "${CMAKE_CURRENT_LIST_DIR}/qbrobotics_driverTargets.cmake") +include(${qbrobotics_driver_TARGETS}) + +check_required_components(qbrobotics_driver) \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_communication.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_communication.cpp new file mode 100644 index 0000000..2e74fa8 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_communication.cpp @@ -0,0 +1,498 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2020, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +namespace qbrobotics_research_api { + +class Communication : public ::testing::Test { + protected: + void SetUp() override { + handler_ = std::make_shared(); + int new_serial_ports_retrieved = handler_->listSerialPorts(serial_ports_); + if (new_serial_ports_retrieved < 0) { + ASSERT_EQ(serial_ports_.size(), 0); + } + ASSERT_EQ(serial_ports_.size(), new_serial_ports_retrieved); // there was no previous data stored + + for (auto const &serial_port : serial_ports_) { + std::vector device_ids; + int device_ids_retrieved = handler_->listConnectedDevices(serial_port.serial_port, device_ids); + if (device_ids_retrieved < 0) { + ASSERT_EQ(device_ids.size(), 0); + } + ASSERT_EQ(device_ids.size(), device_ids_retrieved); + + std::set device_ids_set; + for (auto const &id : device_ids) { + ASSERT_GT(id, 0); + ASSERT_LT(id, 128); + device_ids_set.insert(id); + } + ASSERT_EQ(device_ids_set.size(), device_ids.size()); // check uniqueness + serial_ports_ids_[serial_port.serial_port] = device_ids_set; + + for (auto const &id : device_ids) { + std::string device_name = serial_port.serial_port + ":" + std::to_string(id); + devices_.push_back(std::make_unique(handler_, device_name, serial_port.serial_port, id)); + ASSERT_EQ(devices_.back()->params_->id, id); // parameters are automatically retrieved and stored in `device_[device_name]->params_` + devices_not_init_.push_back(std::make_unique(handler_, device_name, serial_port.serial_port, id, false)); + ASSERT_EQ(devices_not_init_.back()->params_->id, id); // even if params are not initialized, the id is always stored correctly + //TODO: add tests to show that `devices_not_init_` works as fine as `devices_` + } + + for (uint8_t id=127; !fake_device_ && id>0; id--) { // only one serial_port:fake_id couple is initialized + if (!device_ids_set.count(id)) { // not connected device + fake_device_ = std::make_unique(handler_, "fake_dev", serial_port.serial_port, id, false); + } + } + } + } + + void TearDown() override {} + + std::vector serial_ports_; + std::map> serial_ports_ids_; + std::shared_ptr handler_; + std::vector> devices_; + std::vector> devices_not_init_; + std::unique_ptr fake_device_; +}; + +TEST_F(Communication, ListSerialPorts) { + for (auto const &serial_port : serial_ports_) { + EXPECT_GT(serial_port.id_product, 0); + EXPECT_GT(serial_port.id_vendor, 0); + EXPECT_EQ(serial_port.manufacturer, "QB Robotics"); + EXPECT_THAT(serial_port.product, ::testing::EndsWith(serial_port.serial_number)); +#if !defined(_WIN32) + EXPECT_THAT(serial_port.product, ::testing::MatchesRegex("[A-Z]+ [0-9]+")); + EXPECT_THAT(serial_port.serial_number, ::testing::MatchesRegex("[0-9]+")); + EXPECT_THAT(serial_port.serial_port, ::testing::MatchesRegex("^/dev/([^/]+)/?$")); +#else + // MSVC does not support [] and {} in gtest + EXPECT_THAT(serial_port.product, ::testing::MatchesRegex("^\\w+ \\d+$")); + EXPECT_THAT(serial_port.serial_number, ::testing::MatchesRegex("\\d+")); + EXPECT_THAT(serial_port.serial_port, ::testing::MatchesRegex("^COM\\d+$")); +#endif + } +} + +TEST_F(Communication, Ping) { + for (auto const &device : devices_) { + EXPECT_EQ(device->ping(), 0); + } + EXPECT_EQ(fake_device_->ping(), -1); +} + +TEST_F(Communication, GetInfo) { + for (auto const &device : devices_) { + std::string info; + EXPECT_EQ(device->getInfo(0, info), 0); + EXPECT_THAT(info, ::testing::ContainsRegex("Firmware version:.*v?([0-9]+(\\.)){3}")); + } + std::string info; + EXPECT_EQ(fake_device_->getInfo(0, info), -1); +} + +TEST_F(Communication, SensorMeasurements) { + for (auto const &device : devices_) { + std::vector positions; + EXPECT_EQ(device->getPositions(positions), 0); + EXPECT_EQ(positions.size(), 3); + EXPECT_EQ(positions.at(0), 4094); //FIXME: -4098 after meminit in some cases on SoftHand + EXPECT_EQ(positions.at(1), 4094); + EXPECT_EQ(positions.at(2), 4094); + + std::vector velocities; + EXPECT_EQ(device->getVelocities(velocities), 0); + EXPECT_EQ(velocities.size(), 3); + EXPECT_EQ(velocities.at(0), 0); + EXPECT_EQ(velocities.at(1), 0); + EXPECT_EQ(velocities.at(2), 0); + + std::vector accelerations; + EXPECT_EQ(device->getAccelerations(accelerations), 0); + EXPECT_EQ(accelerations.size(), 3); + EXPECT_EQ(accelerations.at(0), 0); + EXPECT_EQ(accelerations.at(1), 0); + EXPECT_EQ(accelerations.at(2), 0); + + std::vector currents; + EXPECT_EQ(device->getCurrents(currents), 0); + EXPECT_EQ(currents.size(), 2); + EXPECT_EQ(currents.at(0), 0); + EXPECT_EQ(currents.at(1), 0); + + EXPECT_EQ(device->getCurrentsAndPositions(currents, positions), 0); + EXPECT_EQ(currents.size(), 2); + EXPECT_EQ(currents.at(0), 0); + EXPECT_EQ(currents.at(1), 0); + EXPECT_EQ(positions.size(), 3); + EXPECT_EQ(positions.at(0), 4094); //FIXME: -4098 after meminit in some cases on SoftHand + EXPECT_EQ(positions.at(1), 127); //FIXME: [[firmware bug]] (-129 or -29825 after meminit in some cases) + EXPECT_EQ(positions.at(2), 4094); + } + std::vector fake_data; + EXPECT_EQ(fake_device_->getPositions(fake_data), -1); + EXPECT_EQ(fake_device_->getVelocities(fake_data), -1); + EXPECT_EQ(fake_device_->getAccelerations(fake_data), -1); + EXPECT_EQ(fake_device_->getCurrents(fake_data), -1); + EXPECT_EQ(fake_device_->getCurrentsAndPositions(fake_data, fake_data), -1); +} + +TEST_F(Communication, ControlReferences) { + for (auto const &device : devices_) { + bool motor_state = true; + EXPECT_EQ(device->getMotorStates(motor_state), 0); + ASSERT_EQ(motor_state, false); // skip if motor is active + + std::vector control_references; + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 4094); //FIXME: 8188 or 14431 after meminit in some cases on SoftHand + EXPECT_EQ(control_references.at(1), 0); // SoftHand + control_references.at(0) = 12345; + control_references.at(1) = 3210; + EXPECT_EQ(device->setControlReferences(control_references), 0); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + control_references.clear(); + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 12345); + EXPECT_EQ(control_references.at(1), 3210); + control_references.at(0) = 0; + control_references.at(1) = 0; + EXPECT_EQ(device->setControlReferences(control_references), 0); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + control_references.clear(); + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 0); + EXPECT_EQ(control_references.at(1), 0); + } + std::vector fake_data; + EXPECT_EQ(fake_device_->getControlReferences(fake_data), -1); + // cannot test setControlReferences failure on fake_device_ because there is no acknowledge +} + +TEST_F(Communication, MotorState) { + for (auto const &device : devices_) { + bool motor_state = true; + EXPECT_EQ(device->getMotorStates(motor_state), 0); + EXPECT_EQ(motor_state, false); + + motor_state = true; + EXPECT_EQ(device->setMotorStates(motor_state), 0); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + motor_state = false; + EXPECT_EQ(device->getMotorStates(motor_state), 0); + EXPECT_EQ(motor_state, true); + motor_state = false; + EXPECT_EQ(device->setMotorStates(motor_state), 0); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + motor_state = true; + EXPECT_EQ(device->getMotorStates(motor_state), 0); + EXPECT_EQ(motor_state, false); + + std::vector control_references; + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 4094); //FIXME: 0 after meminit in some cases on SoftHand + EXPECT_EQ(control_references.at(1), 4094); + control_references.at(0) = 4094; + control_references.at(1) = 0; + EXPECT_EQ(device->setControlReferences(control_references), 0); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + bool fake_data; + EXPECT_EQ(fake_device_->getMotorStates(fake_data), -1); + // cannot test setMotorStates failure on fake_device_ because there is no acknowledge +} + +TEST_F(Communication, DefaultParams) { + for (auto const &device : devices_) { + EXPECT_EQ(device->defaultParamsToMemoryAndArchive(), 0); + + EXPECT_EQ(device->getParamId(device->params_->id), 0); + EXPECT_EQ(device->params_->id, 1); + EXPECT_EQ(device->getParamPositionPID(device->params_->position_pid), 0); + EXPECT_NEAR(device->params_->position_pid.at(0), 0.04, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(1), 0, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(2), 0.06, 0.0001); + EXPECT_EQ(device->getParamCurrentPID(device->params_->current_pid), 0); + EXPECT_NEAR(device->params_->current_pid.at(0), 1.0, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(1), 0.001, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(2), 0, 0.0001); + EXPECT_EQ(device->getParamStartupActivation(device->params_->startup_activation), 0); + EXPECT_EQ(device->params_->startup_activation, false); + EXPECT_EQ(device->getParamInputMode(device->params_->input_mode), 0); + EXPECT_EQ(device->params_->input_mode, 0); + EXPECT_EQ(device->getParamControlMode(device->params_->control_mode), 0); + EXPECT_EQ(device->params_->control_mode, 0); + EXPECT_EQ(device->getParamEncoderResolutions(device->params_->encoder_resolutions), 0); + EXPECT_EQ(device->params_->encoder_resolutions.at(0), 3); + EXPECT_EQ(device->params_->encoder_resolutions.at(1), 3); + EXPECT_EQ(device->params_->encoder_resolutions.at(2), 3); + EXPECT_EQ(device->getParamEncoderOffsets(device->params_->encoder_offsets), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(0), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(1), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(2), 0); + EXPECT_EQ(device->getParamEncoderMultipliers(device->params_->encoder_multipliers), 0); + EXPECT_EQ(device->params_->encoder_multipliers.at(0), 1); + EXPECT_EQ(device->params_->encoder_multipliers.at(1), 1); + EXPECT_EQ(device->params_->encoder_multipliers.at(2), 1); + EXPECT_EQ(device->getParamUsePositionLimits(device->params_->use_position_limits), 0); + EXPECT_EQ(device->params_->use_position_limits, true); + EXPECT_EQ(device->getParamPositionLimits(device->params_->position_limits), 0); + EXPECT_EQ(device->params_->position_limits.at(0), 0); + EXPECT_EQ(device->params_->position_limits.at(1), 19000); + EXPECT_EQ(device->getParamPositionMaxSteps(device->params_->position_max_steps), 0); + EXPECT_EQ(device->params_->position_max_steps.at(0), 0); + EXPECT_EQ(device->getParamCurrentLimit(device->params_->current_limit), 0); + EXPECT_EQ(device->params_->current_limit, 750); + + std::vector control_references, positions; + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 4094); //FIXME: 8188 after meminit in some cases on SoftHand + EXPECT_EQ(control_references.at(1), 0); // SoftHand + EXPECT_EQ(device->getPositions(positions), 0); + EXPECT_EQ(positions.size(), 3); + EXPECT_EQ(positions.at(0), 4094); //FIXME: -4098 after meminit in some cases on SoftHand + EXPECT_EQ(positions.at(1), 4094); + EXPECT_EQ(positions.at(2), 4094); + } + EXPECT_EQ(fake_device_->defaultParamsToMemoryAndArchive(), -1); +} + +TEST_F(Communication, SetParameters) { + for (auto const &device : devices_) { + EXPECT_EQ(device->setParamId(111), 0); // this is the only one which includes storeParamsToMemory + EXPECT_EQ(device->params_->id, 111); + EXPECT_EQ(device->getParamId(device->params_->id), 0); + ASSERT_EQ(device->params_->id, 111); + + EXPECT_EQ(device->setParamPositionPID({0.123, -0.234, 0.0}), 0); + EXPECT_EQ(device->setParamCurrentPID({0.0, -0.999, 0.5}), 0); + EXPECT_EQ(device->setParamStartupActivation(true), 0); + EXPECT_EQ(device->setParamInputMode(1), 0); + EXPECT_EQ(device->setParamControlMode(1), 0); + EXPECT_EQ(device->setParamEncoderResolutions({0,5,2}), 0); + EXPECT_EQ(device->setParamEncoderOffsets({4094, 0, -4000}), 0); + EXPECT_EQ(device->setParamEncoderMultipliers({3.4,-1.1,0.0}), 0); + EXPECT_EQ(device->setParamUsePositionLimits(false), 0); + EXPECT_EQ(device->setParamPositionLimits({-19000, 0}), 0); + EXPECT_EQ(device->setParamPositionMaxSteps({-100, 100}), 0); + EXPECT_EQ(device->setParamCurrentLimit(-1500), 0); + EXPECT_EQ(device->storeParamsToMemory(), 0); + + EXPECT_EQ(device->getParamPositionPID(device->params_->position_pid), 0); + EXPECT_NEAR(device->params_->position_pid.at(0), 0.123, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(1), -0.234, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(2), 0.0, 0.0001); + EXPECT_EQ(device->getParamCurrentPID(device->params_->current_pid), 0); + EXPECT_NEAR(device->params_->current_pid.at(0), 0.0, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(1), -0.999, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(2), 0.5, 0.0001); + EXPECT_EQ(device->getParamStartupActivation(device->params_->startup_activation), 0); + EXPECT_EQ(device->params_->startup_activation, true); + EXPECT_EQ(device->getParamInputMode(device->params_->input_mode), 0); + EXPECT_EQ(device->params_->input_mode, 1); + EXPECT_EQ(device->getParamControlMode(device->params_->control_mode), 0); + EXPECT_EQ(device->params_->control_mode, 1); + EXPECT_EQ(device->getParamEncoderResolutions(device->params_->encoder_resolutions), 0); + EXPECT_EQ(device->params_->encoder_resolutions.at(0), 0); + EXPECT_EQ(device->params_->encoder_resolutions.at(1), 5); + EXPECT_EQ(device->params_->encoder_resolutions.at(2), 2); + EXPECT_EQ(device->getParamEncoderOffsets(device->params_->encoder_offsets), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(0), 4094); + EXPECT_EQ(device->params_->encoder_offsets.at(1), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(2), -4000); + EXPECT_EQ(device->getParamEncoderMultipliers(device->params_->encoder_multipliers), 0); + EXPECT_NEAR(device->params_->encoder_multipliers.at(0), 3.4, 0.0001); + EXPECT_NEAR(device->params_->encoder_multipliers.at(1), -1.1, 0.0001); + EXPECT_NEAR(device->params_->encoder_multipliers.at(2), 0.0, 0.0001); + EXPECT_EQ(device->getParamUsePositionLimits(device->params_->use_position_limits), 0); + EXPECT_EQ(device->params_->use_position_limits, false); + EXPECT_EQ(device->getParamPositionLimits(device->params_->position_limits), 0); + EXPECT_EQ(device->params_->position_limits.at(0), -19000); + EXPECT_EQ(device->params_->position_limits.at(1), 0); + EXPECT_EQ(device->getParamPositionMaxSteps(device->params_->position_max_steps), 0); + EXPECT_EQ(device->params_->position_max_steps.at(0), -100); + EXPECT_EQ(device->params_->position_max_steps.at(1), 100); + EXPECT_EQ(device->getParamCurrentLimit(device->params_->current_limit), 0); + EXPECT_EQ(device->params_->current_limit, -1500); + + std::vector control_references, positions; + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), -15722); //FIXME: [[firmware bug]] (sometimes 8175 or 4081 on SoftHand) + EXPECT_EQ(control_references.at(1), 0); + EXPECT_EQ(device->getPositions(positions), 0); + EXPECT_EQ(positions.size(), 3); + EXPECT_EQ(positions.at(0), -5796); //FIXME: [[firmware bug]] (sometimes -4 or 8188 on SoftHand) + EXPECT_EQ(positions.at(1), -1126); //FIXME: [[firmware bug]] (should be 4094) + EXPECT_EQ(positions.at(2), 0); //FIXME: [[firmware bug]] (should be 0) + } + std::vector param_buffer; + EXPECT_EQ(fake_device_->getParameters(param_buffer), -1); + // cannot test setParameters failure on fake_device_ because there is no acknowledge +} + +TEST_F(Communication, RestoreParams) { + for (auto const &device : devices_) { + EXPECT_EQ(device->restoreParamsFromArchive(), 0); + + EXPECT_EQ(device->getParamId(device->params_->id), 0); + EXPECT_EQ(device->params_->id, 1); + EXPECT_EQ(device->getParamPositionPID(device->params_->position_pid), 0); + EXPECT_NEAR(device->params_->position_pid.at(0), 0.04, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(1), 0, 0.0001); + EXPECT_NEAR(device->params_->position_pid.at(2), 0.06, 0.0001); + EXPECT_EQ(device->getParamCurrentPID(device->params_->current_pid), 0); + EXPECT_NEAR(device->params_->current_pid.at(0), 1.0, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(1), 0.001, 0.0001); + EXPECT_NEAR(device->params_->current_pid.at(2), 0, 0.0001); + EXPECT_EQ(device->getParamStartupActivation(device->params_->startup_activation), 0); + EXPECT_EQ(device->params_->startup_activation, false); + EXPECT_EQ(device->getParamInputMode(device->params_->input_mode), 0); + EXPECT_EQ(device->params_->input_mode, 0); + EXPECT_EQ(device->getParamControlMode(device->params_->control_mode), 0); + EXPECT_EQ(device->params_->control_mode, 0); + EXPECT_EQ(device->getParamEncoderResolutions(device->params_->encoder_resolutions), 0); + EXPECT_EQ(device->params_->encoder_resolutions.at(0), 3); + EXPECT_EQ(device->params_->encoder_resolutions.at(1), 3); + EXPECT_EQ(device->params_->encoder_resolutions.at(2), 3); + EXPECT_EQ(device->getParamEncoderOffsets(device->params_->encoder_offsets), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(0), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(1), 0); + EXPECT_EQ(device->params_->encoder_offsets.at(2), 0); + EXPECT_EQ(device->getParamEncoderMultipliers(device->params_->encoder_multipliers), 0); + EXPECT_EQ(device->params_->encoder_multipliers.at(0), 1); + EXPECT_EQ(device->params_->encoder_multipliers.at(1), 1); + EXPECT_EQ(device->params_->encoder_multipliers.at(2), 1); + EXPECT_EQ(device->getParamUsePositionLimits(device->params_->use_position_limits), 0); + EXPECT_EQ(device->params_->use_position_limits, true); + EXPECT_EQ(device->getParamPositionLimits(device->params_->position_limits), 0); + EXPECT_EQ(device->params_->position_limits.at(0), 0); + EXPECT_EQ(device->params_->position_limits.at(1), 19000); + EXPECT_EQ(device->getParamPositionMaxSteps(device->params_->position_max_steps), 0); + EXPECT_EQ(device->params_->position_max_steps.at(0), 0); + EXPECT_EQ(device->getParamCurrentLimit(device->params_->current_limit), 0); + EXPECT_EQ(device->params_->current_limit, 750); + + std::vector control_references, positions; + EXPECT_EQ(device->getControlReferences(control_references), 0); + EXPECT_EQ(control_references.size(), 2); + EXPECT_EQ(control_references.at(0), 14431); //FIXME: [[firmware bug]] (sometimes 8175 or 8188 on SoftHand) + EXPECT_EQ(control_references.at(1), 0); + EXPECT_EQ(device->getPositions(positions), 0); + EXPECT_EQ(positions.size(), 3); + EXPECT_EQ(positions.at(0), 4094); //FIXME: [[firmware bug]] (sometimes -4098 on SoftHand) + EXPECT_EQ(positions.at(1), 4094); + EXPECT_EQ(positions.at(2), 4094); + } + EXPECT_EQ(fake_device_->restoreParamsFromArchive(), -1); +} + +namespace internal { + +TEST(CommunicationInternal, SwapEndians) { + uint8_t u8{0x01}; + qbroboticsResearchAPI::Communication::swapBytes(u8); + EXPECT_EQ(u8, 0x01); + + uint16_t u16{0x1100}; + qbroboticsResearchAPI::Communication::swapBytes(u16); + EXPECT_EQ(u16, 0x0011); + + uint32_t u32{0x33221100}; + qbroboticsResearchAPI::Communication::swapBytes(u32); + EXPECT_EQ(u32, 0x00112233); + + std::vector u8v{0x01, 0x02, 0x03, 0x04}; + qbroboticsResearchAPI::Communication::swapBytes(*reinterpret_cast(u8v.data())); + EXPECT_EQ(u8v[0], 0x04); + EXPECT_EQ(u8v[1], 0x03); + EXPECT_EQ(u8v[2], 0x02); + EXPECT_EQ(u8v[3], 0x01); + qbroboticsResearchAPI::Communication::swapBytes(*reinterpret_cast(u8v.data() + 1)); + EXPECT_EQ(u8v[0], 0x04); + EXPECT_EQ(u8v[1], 0x02); + EXPECT_EQ(u8v[2], 0x03); + EXPECT_EQ(u8v[3], 0x01); + + qbroboticsResearchAPI::Communication::swapBytes(u8v); + EXPECT_EQ(u8v[0], 0x04); + EXPECT_EQ(u8v[1], 0x02); + EXPECT_EQ(u8v[2], 0x03); + EXPECT_EQ(u8v[3], 0x01); + + std::vector u16v{0x1100, 0x2200, 0x3300, 0x4400}; + qbroboticsResearchAPI::Communication::swapBytes(u16v); + EXPECT_EQ(u16v[0], 0x0011); + EXPECT_EQ(u16v[1], 0x0022); + EXPECT_EQ(u16v[2], 0x0033); + EXPECT_EQ(u16v[3], 0x0044); + + std::vector u32v{0x11567800, 0x22567800, 0x33567800, 0x44567800}; + qbroboticsResearchAPI::Communication::swapBytes(u32v); + EXPECT_EQ(u32v[0], 0x00785611); + EXPECT_EQ(u32v[1], 0x00785622); + EXPECT_EQ(u32v[2], 0x00785633); + EXPECT_EQ(u32v[3], 0x00785644); + + auto u8v_cast = qbroboticsResearchAPI::Communication::vectorCast(u32v); + EXPECT_EQ(u8v_cast[0], 0x11); + EXPECT_EQ(u8v_cast[4], 0x22); + EXPECT_EQ(u8v_cast[8], 0x33); + EXPECT_EQ(u8v_cast[12], 0x44); + + auto u32v_cast = qbroboticsResearchAPI::Communication::vectorCast(u8v); + EXPECT_EQ(u32v_cast[0], 0x01030204); + + auto u32v_cast_swap = qbroboticsResearchAPI::Communication::vectorCastAndSwap(u8v); + EXPECT_EQ(u32v_cast_swap[0], 0x04020301); + auto u8v_swap_cast = qbroboticsResearchAPI::Communication::vectorSwapAndCast(u32v_cast_swap); + EXPECT_EQ(u8v_swap_cast[0], 0x04); + EXPECT_EQ(u8v_swap_cast[1], 0x02); + EXPECT_EQ(u8v_swap_cast[2], 0x03); + EXPECT_EQ(u8v_swap_cast[3], 0x01); +} + +} // namespace internal + +} // namespace qbrobotics_research_api + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_wrapper_communication.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_wrapper_communication.cpp new file mode 100644 index 0000000..76f9bd6 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/qbrobotics-driver/tests/qbrobotics_research_api_wrapper_communication.cpp @@ -0,0 +1,208 @@ +/*** + * Software License Agreement: BSD 3-Clause License + * + * Copyright (c) 2015-2020, qbrobotics® + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + * following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this list of conditions and the + * following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + * following disclaimer in the documentation and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + * products derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE + * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include + +namespace qbrobotics_research_api { + +TEST(Communication, ListSerialPorts) { + char serial_ports[60][255]; + int count = RS485listPorts(serial_ports); + for (int i=0; i(device_ids[j]); + + EXPECT_EQ(commPing(&comm_settings_t, device_id), 0); + + EXPECT_EQ(commInitMem(&comm_settings_t, device_id), 0); + + char motor_state = 0x01; + EXPECT_EQ(commGetActivate(&comm_settings_t, device_id, &motor_state), 0); + EXPECT_EQ(motor_state, 0x00); + + short int positions[3]; + EXPECT_EQ(commGetMeasurements(&comm_settings_t, device_id, positions), 0); + EXPECT_EQ(positions[0], 4094); + EXPECT_EQ(positions[1], 4094); + EXPECT_EQ(positions[2], 4094); + + short int velocities[3]; + EXPECT_EQ(commGetVelocities(&comm_settings_t, device_id, velocities), 0); + EXPECT_EQ(velocities[0], 0); + EXPECT_EQ(velocities[1], 0); + EXPECT_EQ(velocities[2], 0); + + short int accelerations[3]; + EXPECT_EQ(commGetAccelerations(&comm_settings_t, device_id, accelerations), 0); + EXPECT_EQ(accelerations[0], 0); + EXPECT_EQ(accelerations[1], 0); + EXPECT_EQ(accelerations[2], 0); + + short int currents[2]; + EXPECT_EQ(commGetCurrents(&comm_settings_t, device_id, currents), 0); + EXPECT_EQ(currents[0], 0); + EXPECT_EQ(currents[1], 0); + + short int measurements[5]; + EXPECT_EQ(commGetCurrAndMeas(&comm_settings_t, device_id, measurements), 0); + EXPECT_EQ(measurements[0], 0); + EXPECT_EQ(measurements[1], 0); + EXPECT_EQ(measurements[2], 4094); + EXPECT_EQ(measurements[3], 127); //FIXME: firmware error + EXPECT_EQ(measurements[4], 4094); + + short int control_references[2]; + EXPECT_EQ(commGetInputs(&comm_settings_t, device_id, control_references), 0); + EXPECT_EQ(control_references[0], 4094); + EXPECT_EQ(control_references[1], 0); // SoftHand + control_references[0] = 12345; + control_references[1] = 3210; + commSetInputs(&comm_settings_t, device_id, control_references); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + control_references[0] = 0; + control_references[1] = 0; + EXPECT_EQ(commGetInputs(&comm_settings_t, device_id, control_references), 0); + EXPECT_EQ(control_references[0], 12345); + EXPECT_EQ(control_references[1], 3210); + control_references[0] = 0; + control_references[1] = 0; + commSetInputs(&comm_settings_t, device_id, control_references); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + control_references[0] = 1; + control_references[1] = 1; + EXPECT_EQ(commGetInputs(&comm_settings_t, device_id, control_references), 0); + EXPECT_EQ(control_references[0], 0); + EXPECT_EQ(control_references[1], 0); + + motor_state = 0x03; + commActivate(&comm_settings_t, device_id, motor_state); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + motor_state = 0x00; + EXPECT_EQ(commGetActivate(&comm_settings_t, device_id, &motor_state), 0); + EXPECT_EQ(motor_state, 0x03); + motor_state = 0x00; + commActivate(&comm_settings_t, device_id, motor_state); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + motor_state = 0x03; + EXPECT_EQ(commGetActivate(&comm_settings_t, device_id, &motor_state), 0); + EXPECT_EQ(motor_state, 0x00); + + EXPECT_EQ(commGetInputs(&comm_settings_t, device_id, control_references), 0); + EXPECT_EQ(control_references[0], 4094); + EXPECT_EQ(control_references[1], 4094); + control_references[0] = 4094; + control_references[1] = 0; + commSetInputs(&comm_settings_t, device_id, control_references); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + char info[2000]; + EXPECT_EQ(commGetInfo(&comm_settings_t, device_id, INFO_ALL, info), 0); + EXPECT_EQ(std::string(info).size(), 1092); + + uint8_t buffer[2000]; + std::vector param_buffer; + std::vector position_pid; + EXPECT_EQ(commGetParamList(&comm_settings_t, device_id, 0, nullptr, 0, 0, buffer), 0); + param_buffer = std::vector({buffer, buffer+2000}); + qbroboticsResearchAPI::Device::Params::getParameter(2, param_buffer, position_pid); + EXPECT_NEAR(position_pid.at(0), 0.0399932861, 0.0001); + EXPECT_NEAR(position_pid.at(1), 0.0, 0.0001); + EXPECT_NEAR(position_pid.at(2), 0.0599975586, 0.0001); + + float pid_values[3]; + pid_values[0] = 0.1; + pid_values[1] = 0.2; + pid_values[2] = 0.3; + int param_index = 2; + int value_size = 4; + int num_of_values = 3; + commGetParamList(&comm_settings_t, device_id, param_index, pid_values, value_size, num_of_values, nullptr); + + EXPECT_EQ(commStoreParams(&comm_settings_t, device_id), 0); + + EXPECT_EQ(commGetParamList(&comm_settings_t, device_id, 0, nullptr, 0, 0, buffer), 0); + param_buffer = std::vector({buffer, buffer+2000}); + qbroboticsResearchAPI::Device::Params::getParameter(2, param_buffer, position_pid); + EXPECT_NEAR(position_pid.at(0), 0.1, 0.0001); + EXPECT_NEAR(position_pid.at(1), 0.2, 0.0001); + EXPECT_NEAR(position_pid.at(2), 0.3, 0.0001); + + EXPECT_EQ(commRestoreParams(&comm_settings_t, device_id), 0); + } + + closeRS485(&comm_settings_t); + } +} + +//TODO: add tests for non connected device + +} // namespace qbrobotics_research_api + +int main(int argc, char **argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/.gitignore b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/.gitignore new file mode 100644 index 0000000..a134070 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/.gitignore @@ -0,0 +1,5 @@ +.vscode/ +SerialConfig.cmake +*.pro.* +*.autosave +build/ diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CHANGELOG.rst b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CHANGELOG.rst new file mode 100644 index 0000000..ccd32f9 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CHANGELOG.rst @@ -0,0 +1,85 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package serial +^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2015-04-21) +------------------ +* Removed the use of a C++11 feature for compatibility with older browsers. +* Fixed an issue with cross compiling with mingw on Windows. +* Restructured Visual Studio project layout. +* Added include of ``#include `` on OS X (listing of ports). +* Fixed MXE for the listing of ports on Windows. +* Now closes file device if ``reconfigureDevice`` fails (Windows). +* Added the MARK/SPACE parity bit option, also made it optional. + Adding the enumeration values for MARK and SPACE was the only code change to an API header. + It should not affect ABI or API. +* Added support for 576000 baud on Linux. +* Now releases iterator properly in listing of ports code for OS X. +* Fixed the ability to open COM ports over COM10 on Windows. +* Fixed up some documentation about exceptions in ``serial.h``. + +1.2.0 (2014-07-02) +------------------ +* Removed vestigial ``read_cache_`` private member variable from Serial::Serial +* Fixed usage of scoped locks + Previously they were getting destroyed immediately because they were not stored in a temporary scope variable +* Added check of return value from close in Serial::SerialImpl::close () in unix.cc and win.cc +* Added ability to enumerate ports on linux and windows. + Updated serial_example.cc to show example of port enumeration. +* Fixed compile on VS2013 +* Added functions ``waitReadable`` and ``waitByteTimes`` with implemenations for Unix to support high performance reading +* Contributors: Christopher Baker, Craig Lilley, Konstantina Kastanara, Mike Purvis, William Woodall + +1.1.7 (2014-02-20) +------------------ +* Improved support for mingw (mxe.cc) +* Fix compilation warning + See issue `#53 `_ +* Improved timer handling in unix implementation +* fix broken ifdef _WIN32 +* Fix broken ioctl calls, add exception handling. +* Code guards for platform-specific implementations. (when not using cmake / catkin) +* Contributors: Christopher Baker, Mike Purvis, Nicolas Bigaouette, William Woodall, dawid + +1.1.6 (2013-10-17) +------------------ +* Move stopbits_one_point_five to the end of the enum, so that it doesn't alias with stopbits_two. + +1.1.5 (2013-09-23) +------------------ +* Fix license labeling, I put BSD, but the license has always been MIT... +* Added Microsoft Visual Studio 2010 project to make compiling on Windows easier. +* Implemented Serial::available() for Windows +* Update how custom baudrates are handled on OS X + This is taken from the example serial program on Apple's developer website, see: + http://free-pascal-general.1045716.n5.nabble.com/Non-standard-baud-rates-in-OS-X-IOSSIOSPEED-IOCTL-td4699923.html +* Timout settings are now applied by reconfigurePort +* Pass LPCWSTR to CreateFile in Windows impl +* Use wstring for ``port_`` type in Windows impl + +1.1.4 (2013-06-12 00:13:18 -0600) +--------------------------------- +* Timing calculation fix for read and write. + Fixes `#27 `_ +* Update list of exceptions thrown from constructor. +* fix, by Thomas Hoppe + For SerialException's: + * The name was misspelled... + * Use std::string's for error messages to prevent corruption of messages on some platforms +* alloca.h does not exist on OpenBSD either. + +1.1.3 (2013-01-09 10:54:34 -0800) +--------------------------------- +* Install headers + +1.1.2 (2012-12-14 14:08:55 -0800) +--------------------------------- +* Fix buildtool depends + +1.1.1 (2012-12-03) +------------------ +* Removed rt linking on OS X. Fixes `#24 `_. + +1.1.0 (2012-10-24) +------------------ +* Previous history is unstructured and therefore has been truncated. See the commit messages for more info. diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeLists.txt b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeLists.txt new file mode 100644 index 0000000..5b970c7 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeLists.txt @@ -0,0 +1,174 @@ +### +# MIT License +# +# Copyright (c) 2020 Alessandro Tondo +# Copyright (c) 2012 William Woodall, John Harrison +# +# Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated +# documentation files (the "Software"), to deal in the Software without restriction, including without limitation +# the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and +# to permit persons to whom the Software is furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all copies or substantial portions of +# the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO +# THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, +# TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. +# + +cmake_minimum_required(VERSION 3.5.1 FATAL_ERROR) +project(Serial VERSION 2.0.3 LANGUAGES CXX) +message("-- [ ${PROJECT_NAME}] start compiling") +if(APPLE) + find_library(COREFOUNDATION_LIBRARY CoreFoundation) + if (NOT COREFOUNDATION_LIBRARY) + message(FATAL_ERROR "CoreFoundation framework not found.") + endif() + find_library(IOKIT_LIBRARY IOKit) + if (NOT IOKIT_LIBRARY) + message(FATAL_ERROR "IOKit framework not found.") + endif() +elseif(UNIX) + include(GNUInstallDirs) +elseif(WIN32) + if (MSVC_VERSION GREATER_EQUAL "1900") + include(CheckCXXCompilerFlag) + CHECK_CXX_COMPILER_FLAG("/std:c++latest" _cpp_latest_flag_supported) + if (_cpp_latest_flag_supported) + add_compile_options("/std:c++latest") + endif() + endif() + add_definitions(-DUNICODE -D_UNICODE) +else() + message(FATAL_ERROR "Not supported platform.") +endif() + +add_library(Serial + STATIC # [SHARED | STATIC] + src/serial.cpp +) +target_include_directories(Serial + PUBLIC + "$" + PRIVATE + "$" +) +include_directories(Serial SHARED + include +) +if(APPLE) + target_sources(Serial PRIVATE + src/impl/impl_unix.cpp + src/impl/list_ports/list_ports_macos.cpp + ) + target_link_libraries(Serial PRIVATE + "-framework CoreFoundation" + "-framework IOKit" + ) +elseif(UNIX) + target_sources(Serial PRIVATE + src/impl/impl_unix.cpp + src/impl/list_ports/list_ports_linux.cpp + ) +else() # already asserted WIN32 during find_package block + target_sources(Serial PRIVATE + src/impl/impl_win.cpp + src/impl/list_ports/list_ports_win.cpp + ) + target_link_libraries(Serial PRIVATE + setupapi + ) +endif() +# target_compile_features(Serial PUBLIC cxx_std_11) is available only from CMake 3.8 +if(NOT WIN32) + set_target_properties(Serial PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) +endif() +install(TARGETS + Serial + EXPORT SerialConfig + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} +) +export(TARGETS + Serial + NAMESPACE Serial:: + FILE "${CMAKE_CURRENT_SOURCE_DIR}/SerialConfig.cmake" +) +install(EXPORT + SerialConfig + DESTINATION "${CMAKE_CURRENT_SOURCE_DIR}/" + NAMESPACE Serial:: +) + +# Tests +set(TESTS false) # choose if enabling tests or not +if(TESTS) + enable_testing() + find_package(GTest REQUIRED CONFIG) + + add_executable(unix_serial_tests + tests/unix_serial_tests.cpp + ) + target_link_libraries(unix_serial_tests PRIVATE + Serial::Serial + GTest::gtest + GTest::gmock + GTest::gtest_main + ) + if(UNIX) + target_link_libraries(unix_serial_tests PRIVATE + util #FIXME: this is just to use the already existing tests + ) + endif() + set_target_properties(unix_serial_tests PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) + add_test(NAME unix_serial_tests + COMMAND unix_serial_tests + ) + + add_executable(unix_timer_tests + tests/unix_timer_tests.cpp + ) + target_link_libraries(unix_timer_tests PRIVATE + Serial::Serial + GTest::gtest + GTest::gtest_main + ) + set_target_properties(unix_timer_tests PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) + add_test(NAME unix_timer_tests + COMMAND unix_timer_tests + ) + + add_executable(list_ports_tests + tests/list_ports_tests.cpp + ) + target_link_libraries(list_ports_tests PRIVATE + Serial::Serial + GTest::gtest + GTest::gmock + GTest::gtest_main + ) + set_target_properties(list_ports_tests PROPERTIES + CXX_STANDARD 11 + CXX_STANDARD_REQUIRED ON + CXX_EXTENSIONS NO + ) + add_test(NAME list_ports_tests + COMMAND list_ports_tests + ) +endif() diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeSettings.json b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeSettings.json new file mode 100644 index 0000000..dac865a --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CMakeSettings.json @@ -0,0 +1,40 @@ +{ + "configurations": [ + { + "name": "debug-msvc", + "generator": "Ninja", + "configurationType": "Debug", + "inheritEnvironments": [ "msvc_x64_x64" ], + "buildRoot": "${projectDir}\\build\\cmake-build-${name}", + "installRoot": "${projectDir}\\build\\cmake-install-${name}", + "cmakeCommandArgs": "-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON -DBUILD_SHARED_LIBS=ON", + "buildCommandArgs": "", + "ctestCommandArgs": "-C Debug --output-on-failure", + "variables": [ + { + "name": "GTest_DIR", + "value": "C:/Program Files (x86)/googletest-distribution", + "type": "PATH" + } + ] + }, + { + "name": "release-msvc", + "generator": "Ninja", + "configurationType": "Release", + "inheritEnvironments": [ "msvc_x64_x64" ], + "buildRoot": "${projectDir}\\build\\cmake-build-${name}", + "installRoot": "${projectDir}\\build\\cmake-install-${name}", + "cmakeCommandArgs": "-DCMAKE_WINDOWS_EXPORT_ALL_SYMBOLS=ON -DBUILD_SHARED_LIBS=ON", + "buildCommandArgs": "", + "ctestCommandArgs": "-C Release --output-on-failure", + "variables": [ + { + "name": "GTest_DIR", + "value": "C:/Program Files (x86)/googletest-distribution", + "type": "PATH" + } + ] + } + ] +} \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CODE_OF_CONDUCT.md b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CODE_OF_CONDUCT.md new file mode 100644 index 0000000..015d6c2 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/CODE_OF_CONDUCT.md @@ -0,0 +1,76 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to make participation in our project and +our community a harassment-free experience for everyone, regardless of age, body +size, disability, ethnicity, sex characteristics, gender identity and expression, +level of experience, education, socio-economic status, nationality, personal +appearance, race, religion, or sexual identity and orientation. + +## Our Standards + +Examples of behavior that contributes to creating a positive environment +include: + +* Using welcoming and inclusive language +* Being respectful of differing viewpoints and experiences +* Gracefully accepting constructive criticism +* Focusing on what is best for the community +* Showing empathy towards other community members + +Examples of unacceptable behavior by participants include: + +* The use of sexualized language or imagery and unwelcome sexual attention or + advances +* Trolling, insulting/derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or electronic + address, without explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Our Responsibilities + +Project maintainers are responsible for clarifying the standards of acceptable +behavior and are expected to take appropriate and fair corrective action in +response to any instances of unacceptable behavior. + +Project maintainers have the right and responsibility to remove, edit, or +reject comments, commits, code, wiki edits, issues, and other contributions +that are not aligned to this Code of Conduct, or to ban temporarily or +permanently any contributor for other behaviors that they deem inappropriate, +threatening, offensive, or harmful. + +## Scope + +This Code of Conduct applies within all project spaces, and it also applies when +an individual is representing the project or its community in public spaces. +Examples of representing a project or community include using an official +project e-mail address, posting via an official social media account, or acting +as an appointed representative at an online or offline event. Representation of +a project may be further defined and clarified by project maintainers. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported by contacting the project team at alextoind@gmail.com. All +complaints will be reviewed and investigated and will result in a response that +is deemed necessary and appropriate to the circumstances. The project team is +obligated to maintain confidentiality with regard to the reporter of an incident. +Further details of specific enforcement policies may be posted separately. + +Project maintainers who do not follow or enforce the Code of Conduct in good +faith may face temporary or permanent repercussions as determined by other +members of the project's leadership. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, +available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html + +[homepage]: https://www.contributor-covenant.org + +For answers to common questions about this code of conduct, see +https://www.contributor-covenant.org/faq diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/LICENSE b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/LICENSE new file mode 100644 index 0000000..bb7f5e4 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/LICENSE @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2020 Alessandro Tondo +Copyright (c) 2012 William Woodall, John Harrison + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/README.md b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/README.md new file mode 100644 index 0000000..2a24f38 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/README.md @@ -0,0 +1,44 @@ +# Serial library +## Disclaimer + +This repository is a fork of [William Woodall's](https://github.com/wjwwood/serial) and [Alessandro Tondo's](https://github.com/alextoind/serial) serial, who we really thank (together with all the other contributors) for the great job done so far. + +This project can be used as a support tool for the qbrobotics device API (qbdevice-driver-research). + +## Installation + +### Ubuntu Linux +In order to compile the project executes the following commands in the path containing this README file: +``` +mkdir build +cd build +cmake .. +make +``` +Now the **build** folder should contain +**serial** folder, with libSerial.a or libSerial.so library + +### Windows 10 +This project requires: + +- Microsoft Visual Studio 2019 +- Windows SDK 10.0.19041 +- cmake tools +- Developer Command Prompt fo VS 2019 + +Open the **Developer Command Prompt fo VS 2019** and navigate to the folder containing this README file, then executes the following commands: + +``` +mkdir build +cd build +cmake .. +msbuild Serial.sln +``` +The Serial.sln file can be opened and edited with Microsoft Visual Studio 2019. + +--- +**NOTE** + +In order to compile **STATIC** or **DYNAMIC** libraries just move to CMakeLists.txt file and edit the **add_library** line in that file. + +--- diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/SerialConfig.cmake.in b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/SerialConfig.cmake.in new file mode 100644 index 0000000..179c9ee --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/SerialConfig.cmake.in @@ -0,0 +1,6 @@ +@PACKAGE_INIT@ + +set_and_check(Serial_TARGETS "${CMAKE_CURRENT_LIST_DIR}/SerialTargets.cmake") +include(${Serial_TARGETS}) + +check_required_components(Serial) \ No newline at end of file diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/impl/impl.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/impl/impl.h new file mode 100644 index 0000000..7b89fae --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/impl/impl.h @@ -0,0 +1,170 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef SERIAL_IMPL_H +#define SERIAL_IMPL_H + +#include + +#if defined(_WIN32) +#include +#else +#include +#include +#include +#include +#include + +#if defined(__linux__) +#include //TODO: maybe termios2 could be an alternative +#elif defined(__MACH__) +#include +#include +#include +#if defined(MAC_OS_X_VERSION_10_3) && (MAC_OS_X_VERSION_MIN_REQUIRED >= MAC_OS_X_VERSION_10_3) +#include +#endif +//TODO: check if it works properly on macOS +#ifndef TIOCINQ +#ifdef FIONREAD +#define TIOCINQ FIONREAD +#else +#define TIOCINQ 0x541B +#endif +#endif +#endif +#endif + +namespace serial { + +#if defined(_WIN32) +inline std::string escape(const std::string &str) { + std::smatch match; + std::regex_search(str, match, std::regex(R"(^\\\\.\\)")); + if (match.empty()) { + return R"(\\.\)" + str; + } + return str; +} +#endif + +class Serial::SerialImpl { + public: + SerialImpl(std::string port, unsigned long baudrate, Timeout timeout, bytesize_t bytesize, parity_t parity, + stopbits_t stopbits, flowcontrol_t flowcontrol); + + virtual ~SerialImpl(); + + void open(); + + void close(); + + bool isOpen() const; + + size_t available() const; + + bool waitReadable(std::chrono::milliseconds timeout_ms) const; + + bool waitWritable(std::chrono::milliseconds timeout_ms) const; + + void waitByteTimes(size_t count) const; + + size_t read(uint8_t *buf, size_t size = 1); + + size_t write(const uint8_t *data, size_t size); + + void flush() const; + + void flushInput() const; + + void flushOutput() const; + + void sendBreak(int duration_ms) const; + + void setBreak(bool level) const; + + void setModemStatus(uint32_t request, uint32_t command = 0) const; + + void setRTS(bool level) const; + + void setDTR(bool level) const; + + void waitForModemChanges() const; + + uint32_t getModemStatus() const; + + bool getCTS() const; + + bool getDSR() const; + + bool getRI() const; + + bool getCD() const; + + void setPort(const std::string &port); + + std::string getPort() const; + + void setTimeout(const Timeout &timeout); + + Timeout getTimeout() const; + + void setBaudrate(unsigned long baudrate); + + unsigned long getBaudrate() const; + + void setBytesize(bytesize_t bytesize); + + bytesize_t getBytesize() const; + + void setParity(parity_t parity); + + parity_t getParity() const; + + void setStopbits(stopbits_t stopbits); + + stopbits_t getStopbits() const; + + void setFlowcontrol(flowcontrol_t flowcontrol); + + flowcontrol_t getFlowcontrol() const; + + protected: + void reconfigurePort(); + + private: +#if !defined(_WIN32) + int fd_; +#else + HANDLE fd_; +#endif + std::string port_; + bool is_open_; + Timeout timeout_; + unsigned long baudrate_; + parity_t parity_; + bytesize_t bytesize_; + stopbits_t stopbits_; + flowcontrol_t flowcontrol_; +}; +} // namespace serial + +#endif // SERIAL_IMPL_H diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/serial.h b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/serial.h new file mode 100644 index 0000000..e6e55a5 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/include/serial/serial.h @@ -0,0 +1,630 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#ifndef SERIAL_H +#define SERIAL_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace serial { + +/*** + * Enumeration defines the possible bytesizes for the serial port. + */ +typedef enum { + fivebits = 5, + sixbits = 6, + sevenbits = 7, + eightbits = 8 +} bytesize_t; + +/*** + * Enumeration defines the possible parity types for the serial port. + */ +typedef enum { + parity_none = 0, + parity_odd = 1, + parity_even = 2, + parity_mark = 3, + parity_space = 4 +} parity_t; + +/*** + * Enumeration defines the possible stopbit types for the serial port. + */ +typedef enum { + stopbits_one = 1, + stopbits_two = 2, + stopbits_one_point_five +} stopbits_t; + +/*** + * Enumeration defines the possible flowcontrol types for the serial port. + */ +typedef enum { + flowcontrol_none = 0, + flowcontrol_software, + flowcontrol_hardware +} flowcontrol_t; + +/*** + * Class that provides a portable serial port interface. + */ +class Serial { + public: + class Timeout { + public: + /*** + * Create a non-blocking communication mode (since the Timeout structure is filled with 0). + */ + Timeout() = default; + + /*** + * Create a simplified constant-time Timeout structure (@sa Timeout below). + * @param read_write_constant A constant used to calculate the total timeout period for read/write operations. + */ + explicit Timeout(uint32_t read_write_constant) + // the ugly parentheses around `(std::max)()` are required on Windows (this is to avoid to `#undef max` or to `#define NOMINMAX`) + : Timeout((std::numeric_limits::max)(), read_write_constant, 0, read_write_constant, 0) {} + + /*** + * Create a Timeout structure where all the parameters are expressed in milliseconds and refer to the documentation + * of @p COMMTIMEOUTS (https://docs.microsoft.com/en-us/windows/win32/api/winbase/ns-winbase-commtimeouts).@n + * @param inter_byte The maximum time allowed to elapse before the arrival of the next byte. + * @param read_constant A constant used to calculate the total timeout period for read operations. + * @param read_multiplier The multiplier used to calculate the total timeout period for read operations. + * @param write_constant A constant used to calculate the total timeout period for write operations. + * @param write_multiplier The multiplier used to calculate the total timeout period for write operations. + */ + explicit Timeout(uint32_t inter_byte, uint32_t read_constant, uint32_t read_multiplier, uint32_t write_constant, uint32_t write_multiplier) + : inter_byte_(inter_byte), + read_constant_(read_constant), + read_multiplier_(read_multiplier), + write_constant_(write_constant), + write_multiplier_(write_multiplier) {} + + ~Timeout() = default; + + std::chrono::milliseconds getInterByte() { return inter_byte_; } + uint32_t getInterByteMilliseconds() { return inter_byte_.count(); } + std::chrono::milliseconds getReadConstant() { return read_constant_; } + uint32_t getReadConstantMilliseconds() { return read_constant_.count(); } + std::chrono::milliseconds getReadMultiplier() { return read_multiplier_; } + uint32_t getReadMultiplierMilliseconds() { return read_multiplier_.count(); } + std::chrono::milliseconds getWriteConstant() { return write_constant_; } + uint32_t getWriteConstantMilliseconds() { return write_constant_.count(); } + std::chrono::milliseconds getWriteMultiplier() { return write_multiplier_; } + uint32_t getWriteMultiplierMilliseconds() { return write_multiplier_.count(); } + + /*** + * @param size The byte size expected to be read. + * @return The next timeout deadline for the current read settings and the given byte size. + */ + std::chrono::steady_clock::time_point getReadDeadline(size_t size = 0) { + return std::chrono::steady_clock::now() + read_constant_ + read_multiplier_*size; + } + + /*** + * @param size The byte size expected to be written. + * @return The next timeout deadline for the current write settings and the given byte size. + */ + std::chrono::steady_clock::time_point getWriteDeadline(size_t size = 0) { + return std::chrono::steady_clock::now() + write_constant_ + write_multiplier_*size; + } + + /*** + * @param deadline The read/write timeout deadline. + * @return The remaining microseconds w.r.t. the given deadline. + * @sa getReadDeadline, getWriteDeadline + */ + static std::chrono::microseconds remainingMicroseconds(std::chrono::steady_clock::time_point deadline) { + return std::chrono::duration_cast(deadline - std::chrono::steady_clock::now()); + } + + private: + std::chrono::duration inter_byte_; + std::chrono::duration read_constant_; + std::chrono::duration read_multiplier_; + std::chrono::duration write_constant_; + std::chrono::duration write_multiplier_; + }; + + /*** + * Create a Serial object and open the serial port if its name is provided. If not, it remains closed until + * @p Serial::open is called. + * @param port_name The path of the serial port, e.g. 'COM1' on Windows and '/dev/ttyS0' on Linux. + * @param baudrate The baud rate of the serial communication. + * @param timeout The timeout conditions of the serial communication. @sa Serial::Timeout + * @param bytesize The payload size of the serial communication (default is @p eightbits). @sa bytesize_t + * @param parity The parity method of the serial communication (default is @p parity_none). @sa parity_t + * @param stopbits The number of stop bits of the serial communication (default is @p stopbits_one). @sa stopbits_t + * @param flowcontrol The type of flow control of the serial communication (default is @p flowcontrol_none). @sa flowcontrol_t + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + explicit Serial(const std::string &port_name = "", uint32_t baudrate = 9600, Timeout timeout = Timeout(), + bytesize_t bytesize = eightbits, parity_t parity = parity_none, stopbits_t stopbits = stopbits_one, + flowcontrol_t flowcontrol = flowcontrol_none); + + virtual ~Serial(); + + Serial(const Serial &) = delete; + Serial &operator=(const Serial &) = delete; + + /*** + * Open the serial port as long as the port name is set into the Serial object (do nothing if already open).@n + * If the port name is provided to the constructor then an explicit call to open is not needed. + * @sa Serial::Serial + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void open(); + + /*** + * @return @p true if the port is open. + */ + bool isOpen() const; + + /*** + * Close the serial port. + * @throw serial::SerialIOException + */ + void close(); + + /*** + * @return The number of characters available in the input buffer. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t available(); + + /*** + * Block until the port is in a readable state or timeout @p read_constant_ milliseconds have elapsed. + * @note Not implemented on Windows. + * @return @p true if the port is in a readable state. + * @throw serial::SerialException (only on Windows systems) + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool waitReadable(); + + /*** + * Block until the port is in a writable state or timeout @p write_constant_ milliseconds have elapsed. + * @note Not implemented on Windows. + * @return @p true if the port is in a writable state. + * @throw serial::SerialException (only on Windows systems) + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool waitWritable(); + + /*** + * Block for a period of time corresponding to the transmission time of count characters at present serial settings. + * This may be used in conjunction with @p Serial::waitReadable to read larger blocks of data from the port. + * @sa Serial::waitReadable + */ + void waitByteTimes(size_t count); + + /*** + * Read at most the given amount of bytes from the serial port and store them into the given buffer.@n + * This method returns as soon as @p size bytes are read or when the timeout triggers (either when the inter byte or + * the total timeouts expire, @sa Serial::Timeout). + * @param buffer The array of at least the given size (the caller must size it accordingly) where the data is stored. + * @param size The number of bytes that should be read. + * @return The number of bytes actually read. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t read(uint8_t *buffer, size_t size); + + /*** + * Read at most the given amount of bytes from the serial port and store them into the given buffer.@n + * This method returns as soon as @p size bytes are read or when the timeout triggers (either when the inter byte or + * the total timeouts expire, @sa Serial::Timeout). + * @param buffer The vector where the data is stored. + * @param size The number of bytes that should be read. + * @return The number of bytes actually read. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t read(std::vector &buffer, size_t size = 1); + + /*** + * Read at most the given amount of characters from the serial port and store them into the given buffer.@n + * This method returns as soon as @p size characters are read or when the timeout triggers (either when the inter + * byte or the total timeouts expire, @sa Serial::Timeout). + * @param buffer The string where the data is stored. + * @param size The number of characters that should be read. + * @return The number of characters actually read. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t read(std::string &buffer, size_t size = 1); + + /*** + * Read at most the given amount of characters from the serial port and return them as string.@n + * This method returns as soon as @p size characters are read or when the timeout triggers (either when the inter + * byte or the total timeouts expire, @sa Serial::Timeout). + * @param size The number of characters that should be read. + * @return The string where the data is stored. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + std::string read(size_t size = 1); + + /*** + * Read characters from the serial port until the given delimiter is found and store them into the given buffer.@n + * This method returns as soon as the given delimiter is found or @p size characters are read or when the timeout + * triggers (either when the inter byte or the total timeouts expire, @sa Serial::Timeout). + * @param line The string where the data is stored. + * @param size The maximum number of characters that can be read. + * @param eol The string to match against for the EOL. + * @return The number of characters actually read. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t readline(std::string &line, size_t size = 65536, const std::string &eol = "\n"); + + /*** + * Read characters from the serial port until the given delimiter is found and store them into the given buffer.@n + * This method returns as soon as the given delimiter is found or @p size characters are read or when the timeout + * triggers (either when the inter byte or the total timeouts expire, @sa Serial::Timeout). + * @param size The maximum number of characters that can be read. + * @param eol The string to match against for the EOL. + * @return The string where the data is stored. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + std::string readline(size_t size = 65536, const std::string &eol = "\n"); + + /*** + * Read multiple lines from the serial port (identified by the given delimiter) and store them into the given vector + * (one line per element of the vector).@n + * This method returns when @p size characters are read or when the timeout triggers (either when the inter byte or + * the total timeouts expire, @sa Serial::Timeout). + * @param size The maximum number of characters that can be read. + * @param eol The string to match against for the EOL. + * @return The vector of strings where the data lines is stored. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + std::vector readlines(size_t size = 65536, const std::string &eol = "\n"); + + /*** + * Write at most the given amount of bytes to the serial port.@n + * This method returns as soon as @p size bytes are written or when the timeout triggers (either when the inter byte + * or the total timeouts expire, @sa Serial::Timeout).@n + * @param data The array of at least the given size (the caller must size it accordingly) where the data is stored. + * @param size The number of bytes that should be written. + * @return The number of bytes actually written. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t write(const uint8_t *data, size_t size); + + /*** + * Write at most the given amount of bytes to the serial port.@n + * This method returns as soon as @p data.size() bytes are written or when the timeout triggers (either when the + * inter byte or the total timeouts expire, @sa Serial::Timeout). + * @param data The vector where the data is stored. + * @return The number of bytes actually written. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t write(const std::vector &data); + + /*** + * Write at most the given amount of characters to the serial port.@n + * This method returns as soon as @p data.size() characters are written or when the timeout triggers (either when the + * inter byte or the total timeouts expire, @sa Serial::Timeout). + * @param data The string where the data is stored. + * @return The number of bytes actually written. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + size_t write(const std::string &data); + + /*** + * Set the serial port identifier. + * @param port_name The path of the serial port, e.g. 'COM1' on Windows and '/dev/ttyS0' on Linux. + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setPort(const std::string &port_name); + + /*** + * @return The serial port identifier. + */ + std::string getPort() const; + + /*** + * Sets the timeout for reads and writes using the Timeout struct. + * @param timeout A Timeout struct containing the inter byte timeout, and read/write timeout constants and multipliers. + * @sa Serial::Timeout + * @throw serial::SerialIOException (only on Windows systems) + * @throw serial::SerialInvalidArgumentException (only on Windows systems) + */ + void setTimeout(const Timeout &timeout); + + /*** + * Sets the timeouts for reads and writes. + * @param inter_byte The maximum time allowed to elapse before the arrival of the next byte. + * @param read_constant A constant used to calculate the total timeout period for read operations. + * @param read_multiplier The multiplier used to calculate the total timeout period for read operations. + * @param write_constant A constant used to calculate the total timeout period for write operations. + * @param write_multiplier The multiplier used to calculate the total timeout period for write operations. + * @sa Serial::Timeout + * @throw serial::SerialIOException (only on Windows systems) + * @throw serial::SerialInvalidArgumentException (only on Windows systems) + */ + void setTimeout(uint32_t inter_byte, uint32_t read_constant, uint32_t read_multiplier, uint32_t write_constant, uint32_t write_multiplier); + + /*** + * @return A Timeout struct containing the inter byte timeout, and read/write timeout constants and multipliers. + * @sa Serial::Timeout + */ + Timeout getTimeout() const; + + /*** + * Sets the baudrate for the serial port. + * @param baudrate The baud rate of the serial communication. + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setBaudrate(uint32_t baudrate); + + /*** + * @return The baud rate of the serial communication. + */ + uint32_t getBaudrate() const; + + /*** + * Sets the byte size for the serial port. + * @param bytesize The payload size of the serial communication. + * @sa bytesize_t + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setBytesize(bytesize_t bytesize); + + /*** + * @return The payload size of the serial communication. + * @sa bytesize_t + */ + bytesize_t getBytesize() const; + + /*** + * Sets the parity for the serial port. + * @param parity The parity method of the serial communication. + * @sa parity_t + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setParity(parity_t parity); + + /*** + * @return The parity method of the serial communication. + * @sa parity_t + */ + parity_t getParity() const; + + /*** + * Sets the stop bits for the serial port. + * @param stopbits The number of stop bits of the serial communication. + * @sa stopbits_t + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setStopbits(stopbits_t stopbits); + + /*** + * @return The number of stop bits of the serial communication. + * @sa stopbits_t + */ + stopbits_t getStopbits() const; + + /*** + * Sets the flow control for the serial port. + * @param flowcontrol The type of flow control of the serial communication. + * @sa flowcontrol_t + * @throw serial::SerialIOException + * @throw serial::SerialInvalidArgumentException + */ + void setFlowcontrol(flowcontrol_t flowcontrol); + + /*** + * @return The type of flow control of the serial communication. + * @sa flowcontrol_t + */ + flowcontrol_t getFlowcontrol() const; + + /*** + * Flush the input and output buffers of the serial communication. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void flush(); + + /*** + * Flush the input buffer of the serial communication. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void flushInput(); + + /*** + * Flush the output buffer of the serial communication. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void flushOutput(); + + /*** + * Sends the serial break signal, see @p tcsendbreak(). + * @note Not implemented on Windows. + * @throw serial::SerialException (only on Windows systems) + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void sendBreak(int duration); + + /*** + * Set the break condition to the given level (default is @p true, high). + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void setBreak(bool level = true); + + /*** + * Set the RTS handshaking line to the given level (default is @p true, high). + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void setRTS(bool level = true); + + /*** + * Set the DTR handshaking line to the given level (default is @p true, high). + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void setDTR(bool level = true); + + /*** + * Blocks until CTS, DSR, RI, CD changes or something interrupts it. + * @return @p true if one of the lines changed, @p false otherwise. + * @throw serial::SerialException (#ifndef TIOCMIWAIT on unix systems) + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + void waitForModemChanges(); + + /*** + * @return The current status of the CTS line. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool getCTS(); + + /*** + * @return The current status of the DSR line. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool getDSR(); + + /*** + * @return The current status of the RI line. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool getRI(); + + /*** + * @return The current status of the CD line. + * @throw serial::SerialIOException + * @throw serial::SerialPortNotOpenException + */ + bool getCD(); + + private: + class SerialImpl; + std::unique_ptr pimpl_; + std::mutex read_mutex_; + std::mutex write_mutex_; + + size_t readline_(std::string &line, size_t size = 65536, const std::string &eol = "\n"); // core method which does not lock on mutex +}; + +class SerialException : public std::runtime_error { + public: + SerialException() : SerialException("generic fault") {} + explicit SerialException(const std::string &what_arg) : std::runtime_error("Serial Exception: " + what_arg + ".") {} +}; + +class SerialInvalidArgumentException : public std::invalid_argument { + public: + SerialInvalidArgumentException() : SerialInvalidArgumentException("generic fault") {} + explicit SerialInvalidArgumentException(const std::string &what_arg) : std::invalid_argument("Serial Invalid Argument Exception: " + what_arg + ".") {} +}; + +class SerialIOException : public std::runtime_error { + public: + SerialIOException() : SerialIOException("generic fault") {} + explicit SerialIOException(const std::string &what_arg) : std::runtime_error("Serial IO Exception: " + what_arg + "'.") {} + explicit SerialIOException(const std::string &what_arg, uint32_t error) : std::runtime_error("Serial IO Exception: " + what_arg + ", error has been set to '" + std::to_string(error) + "'.") {} +}; + +class SerialPortNotOpenException : public std::runtime_error { + public: + SerialPortNotOpenException() : std::runtime_error("Serial Port Not Open Exception.") {} +}; + +/*** + * Structure that describes a serial device. + */ +class PortInfo { + public: + PortInfo() = default; + ~PortInfo() = default; + + uint16_t busnum {0}; + uint16_t devnum {0}; + uint16_t id_product {0}; + uint16_t id_vendor {0}; + std::string manufacturer; + std::string product; + std::string serial_number; + std::string serial_port; + + /*** + * Fill all the @p PortInfo members with the data from the given serial port node (if found in the system).@n + * If the serial port is not a USB device, only the @p serial_port field is filled. + * @param serial_port_name The serial port name to search for, e.g. 'COM1' on Windows and '/dev/ttyS0' on Linux. + * @return @p 0 on success, @p -1 otherwise. + */ + int getPortInfo(const std::string &serial_port_name); +}; + +/*** + * Return through the given vector the @p PortInfo of all the serial port connected to the system. + * @param serial_ports The vector of serial port info. + * @return @p 0 on success, @p -1 otherwise. + * @sa PortInfo + */ +int getPortsInfo(std::vector &serial_ports); + +/*** + * Return through the given vector the names of all the serial port connected to the system. + * @param serial_port_names The vector of serial port names. + * @return @p 0 on success, @p -1 otherwise. + */ +int getPortsList(std::vector &serial_port_names); + +} // namespace serial + +#endif diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/serial.pro b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/serial.pro new file mode 100644 index 0000000..ffc0e79 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/serial.pro @@ -0,0 +1,21 @@ +TEMPLATE = lib +CONFIG += c++latest +CONFIG += staticlib +#DEFINES += MAKE_TEST_LIB +#VERSION = 1.0.0 +TARGET = libserial + +INCLUDEPATH = include +DEPENDPATH = include + +SOURCES = src/serial.cpp + +win32 { + SOURCES += src/impl/impl_win.cpp \ + src/impl/list_ports/list_ports_win.cpp + LIBS += -lsetupapi +} +unix { + SOURCES += src/impl/impl_unix.cpp \ + src/impl/list_ports/list_ports_linux.cpp +} diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_unix.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_unix.cpp new file mode 100644 index 0000000..d4a53cb --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_unix.cpp @@ -0,0 +1,640 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#if !defined(_WIN32) + +#include + +using namespace serial; + +Serial::SerialImpl::SerialImpl(std::string port, unsigned long baudrate, Timeout timeout, bytesize_t bytesize, + parity_t parity, stopbits_t stopbits, flowcontrol_t flowcontrol) + : port_(std::move(port)), + fd_(-1), + is_open_(false), + timeout_(timeout), + baudrate_(baudrate), + parity_(parity), + bytesize_(bytesize), + stopbits_(stopbits), + flowcontrol_(flowcontrol) { + if (!port_.empty()) { + open(); + } +} + +Serial::SerialImpl::~SerialImpl() { + close(); +} + +void Serial::SerialImpl::open() { + if (port_.empty()) { + throw SerialInvalidArgumentException("serial port is empty."); + } + if (is_open_) { + return; + } + + fd_ = ::open(port_.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK | O_CLOEXEC); + if (fd_ == -1) { + if (errno == EINTR) { + open(); // Recurse because this is a recoverable error + return; + } + throw SerialIOException("failure during ::open()", errno); + } + + // prevent multiple openings + if (::ioctl(fd_, TIOCEXCL) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } + + // set communication as BLOCKING + + if(::fcntl(fd_, F_SETFL, 0) == -1) { + throw SerialIOException("failure during ::fcntl()", errno); + } + + reconfigurePort(); + is_open_ = true; +} + +void Serial::SerialImpl::reconfigurePort() { + if (fd_ == -1) { + throw SerialInvalidArgumentException("invalid file descriptor"); + } + + struct termios options{0}; + if (::tcgetattr(fd_, &options) == -1) { + throw SerialIOException("failure during ::tcgetattr()", errno); + } + + // input modes + options.c_iflag &= ~(IGNBRK | BRKINT | IGNPAR | PARMRK | INPCK | ISTRIP | INLCR | IGNCR | ICRNL | IXON | IXOFF | IXANY); +#ifdef IUCLC + options.c_iflag &= ~IUCLC; +#endif +#ifdef IMAXBEL + options.c_iflag &= ~IMAXBEL; +#endif + + // output modes + options.c_oflag &= ~(OPOST | ONLCR | OCRNL | ONOCR | ONLRET | OFILL); +#ifdef OLCUC + options.c_oflag &= ~OLCUC; +#endif +#ifdef NLDLY + options.c_oflag &= ~NLDLY; +#endif +#ifdef CRDLY + options.c_oflag &= ~CRDLY; +#endif +#ifdef TABDLY + options.c_oflag &= ~TABDLY; +#endif +#ifdef BSDLY + options.c_oflag &= ~BSDLY; +#endif +#ifdef VTDLY + options.c_oflag &= ~VTDLY; +#endif +#ifdef FFDLY + options.c_oflag &= ~FFDLY; +#endif + + // control modes + options.c_cflag &= (CSIZE | CSTOPB | PARENB | PARODD); + options.c_cflag |= (CREAD | HUPCL | CLOCAL); +#ifdef CMSPAR + options.c_cflag &= ~CMSPAR; +#endif +#ifdef CRTSCTS + options.c_cflag &= ~CRTSCTS; +#endif + + // local modes + options.c_lflag &= ~(ISIG | ICANON | ECHO | ECHOE | ECHOK | ECHONL | IEXTEN); +#ifdef ECHOCTL + options.c_lflag &= ~ECHOCTL; +#endif +#ifdef ECHOPRT + options.c_lflag &= ~ECHOPRT; +#endif +#ifdef ECHOKE + options.c_lflag &= ~ECHOKE; +#endif + + // control characters + options.c_cc[VMIN] = 0; + options.c_cc[VTIME] = 0; + + struct serial_struct serinfo; + + ::ioctl(fd_, TIOCGSERIAL, &serinfo); + serinfo.flags |= ASYNC_LOW_LATENCY; + ::ioctl(fd_, TIOCSSERIAL, &serinfo); + + bool use_custom_baudrate = false; + speed_t baudrate = 0; + switch (baudrate_) { +#ifdef B0 + case 0: baudrate = B0; break; +#endif +#ifdef B50 + case 50: baudrate = B50; break; +#endif +#ifdef B75 + case 75: baudrate = B75; break; +#endif +#ifdef B110 + case 110: baudrate = B110; break; +#endif +#ifdef B134 + case 134: baudrate = B134; break; +#endif +#ifdef B150 + case 150: baudrate = B150; break; +#endif +#ifdef B200 + case 200: baudrate = B200; break; +#endif +#ifdef B300 + case 300: baudrate = B300; break; +#endif +#ifdef B600 + case 600: baudrate = B600; break; +#endif +#ifdef B1200 + case 1200: baudrate = B1200; break; +#endif +#ifdef B1800 + case 1800: baudrate = B1800; break; +#endif +#ifdef B2400 + case 2400: baudrate = B2400; break; +#endif +#ifdef B4800 + case 4800: baudrate = B4800; break; +#endif +#ifdef B7200 + case 7200: baudrate = B7200; break; +#endif +#ifdef B9600 + case 9600: baudrate = B9600; break; +#endif +#ifdef B14400 + case 14400: baudrate = B14400; break; +#endif +#ifdef B19200 + case 19200: baudrate = B19200; break; +#endif +#ifdef B28800 + case 28800: baudrate = B28800; break; +#endif +#ifdef B57600 + case 57600: baudrate = B57600; break; +#endif +#ifdef B76800 + case 76800: baudrate = B76800; break; +#endif +#ifdef B38400 + case 38400: baudrate = B38400; break; +#endif +#ifdef B115200 + case 115200: baudrate = B115200; break; +#endif +#ifdef B128000 + case 128000: baudrate = B128000; break; +#endif +#ifdef B153600 + case 153600: baudrate = B153600; break; +#endif +#ifdef B230400 + case 230400: baudrate = B230400; break; +#endif +#ifdef B256000 + case 256000: baudrate = B256000; break; +#endif +#ifdef B460800 + case 460800: baudrate = B460800; break; +#endif +#ifdef B500000 + case 500000: baudrate = B500000; break; +#endif +#ifdef B576000 + case 576000: baudrate = B576000; break; +#endif +#ifdef B921600 + case 921600: baudrate = B921600; break; +#endif +#ifdef B1000000 + case 1000000: baudrate = B1000000; break; +#endif +#ifdef B1152000 + case 1152000: baudrate = B1152000; break; +#endif +#ifdef B1500000 + case 1500000: baudrate = B1500000; break; +#endif +#ifdef B2000000 + case 2000000: baudrate = B2000000; break; +#endif +#ifdef B2500000 + case 2500000: baudrate = B2500000; break; +#endif +#ifdef B3000000 + case 3000000: baudrate = B3000000; break; +#endif +#ifdef B3500000 + case 3500000: baudrate = B3500000; break; +#endif +#ifdef B4000000 + case 4000000: baudrate = B4000000; break; +#endif + default: + baudrate = baudrate_; + use_custom_baudrate = true; + } + + if (!use_custom_baudrate) { + if (::cfsetispeed(&options, baudrate) == -1) { + throw SerialIOException("failure during ::cfsetispeed()", errno); + } + if (::cfsetospeed(&options, baudrate) == -1) { + throw SerialIOException("failure during ::cfsetospeed()", errno); + } + } + + switch (bytesize_) { + case eightbits: options.c_cflag |= CS8; break; + case sevenbits: options.c_cflag |= CS7; break; + case sixbits: options.c_cflag |= CS6; break; + case fivebits: options.c_cflag |= CS5; break; + default: + throw SerialInvalidArgumentException("invalid byte size"); + } + + switch (stopbits_) { + case stopbits_one: options.c_cflag &= ~CSTOPB; break; + case stopbits_one_point_five: // there is no POSIX support for 1.5 stop bit + case stopbits_two: options.c_cflag |= CSTOPB; break; + default: + throw SerialInvalidArgumentException("invalid stop bit"); + } + + switch (parity_) { + case parity_none: options.c_iflag |= IGNPAR; break; + case parity_even: options.c_cflag |= PARENB; break; + case parity_odd: options.c_cflag |= (PARENB | PARODD); break; +#ifdef CMSPAR + case parity_mark: options.c_cflag |= (PARENB | PARODD | CMSPAR); break; + case parity_space: options.c_cflag |= (PARENB | CMSPAR); break; +#endif + default: + throw SerialInvalidArgumentException("invalid parity"); + } + + switch (flowcontrol_) { + case flowcontrol_none: break; + case flowcontrol_software: options.c_iflag |= (IXON | IXOFF | IXANY); break; +#ifdef CRTSCTS + case flowcontrol_hardware: options.c_cflag |= (CRTSCTS); break; +#endif + default: + throw SerialInvalidArgumentException("invalid flow control"); + } + + // activate settings + if (::tcsetattr(fd_, TCSANOW, &options) != 0) { + throw SerialIOException("failure during ::tcsetattr()", errno); + } + + if (use_custom_baudrate) { +#if defined(__APPLE__) + if (::ioctl(fd_, IOSSIOSPEED, &baudrate) == -1) { + throw SerialIOException("failure during ::ioctl(IOSSIOSPEED)", errno); + } +#elif defined(__linux__) && defined (TIOCSSERIAL) //TODO: maybe termios2 could be an alternative + struct serial_struct serial {0}; + if (::ioctl(fd_, TIOCGSERIAL, &serial) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } + + //TODO: check ASYNC_LOW_LATENCY + serial.flags &= ~ASYNC_SPD_MASK; + serial.flags |= ASYNC_SPD_CUST; + serial.custom_divisor = static_cast((serial.baud_base + baudrate_ / 2) / baudrate_); + int closest_baudrate = serial.baud_base / serial.custom_divisor; + if (std::abs(static_cast(closest_baudrate - baudrate_)) < 0.02) { + throw SerialIOException("cannot set baudrate to " + std::to_string(baudrate_) + " (the closest possible value is " + std::to_string(closest_baudrate) + ")"); + } + + if (::ioctl(fd_, TIOCSSERIAL, &serial) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } +#else + throw SerialInvalidArgumentException("custom baudrates are not supported on current OS"); +#endif + } +} + +void Serial::SerialImpl::close() { + if (is_open_ && fd_ != -1 && ::close(fd_) != 0) { + throw SerialIOException("failure during ::close()", errno); + } + fd_ = -1; + is_open_ = false; +} + +bool Serial::SerialImpl::isOpen() const { + return is_open_; +} + +size_t Serial::SerialImpl::available() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + int count = 0; + if (::ioctl(fd_, TIOCINQ, &count) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } + return static_cast(count); +} + +bool waitOnPoll(std::chrono::milliseconds timeout_ms, std::unique_ptr fds) { + int r = ::poll(fds.get(), 1, timeout_ms.count()); + if (r < 0 && errno != EINTR) { + throw SerialIOException("failure during ::poll()", errno); + } + if (fds->revents == POLLERR || fds->revents == POLLHUP || fds->revents == POLLNVAL) { + throw SerialIOException("failure during ::poll(), revents has been set to '" + std::to_string(fds->revents) + "'."); + } + return r > 0; +} + +bool Serial::SerialImpl::waitReadable(std::chrono::milliseconds timeout_ms) const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + return waitOnPoll(timeout_ms, std::unique_ptr(new pollfd{fd_, POLLIN, 0})); +} + +bool Serial::SerialImpl::waitWritable(std::chrono::milliseconds timeout_ms) const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + return waitOnPoll(timeout_ms, std::unique_ptr(new pollfd{fd_, POLLOUT, 0})); +} + +void Serial::SerialImpl::waitByteTimes(size_t count) const { + auto start = std::chrono::steady_clock::now(); + uint32_t bit_time_ns = 1e9 / baudrate_; + uint32_t byte_time_ns = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_); + byte_time_ns += stopbits_ == stopbits_one_point_five ? -1.5*bit_time_ns : 0; // stopbits_one_point_five is 3 + std::this_thread::sleep_until(start + std::chrono::nanoseconds(byte_time_ns * count)); +} + +size_t Serial::SerialImpl::read(uint8_t *buf, size_t size) { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + auto read_deadline = timeout_.getReadDeadline(size); + size_t total_bytes_read = ::read(fd_, buf, size); + if (total_bytes_read == -1) { + throw SerialIOException("failure during ::read()", errno); + } + while (total_bytes_read < size) { + auto remaining_time = Timeout::remainingMicroseconds(read_deadline); + if (remaining_time.count() <= 0) { + break; + } + if (waitReadable(std::min(std::chrono::duration_cast(remaining_time), timeout_.getInterByte()))) { + if (size - total_bytes_read > 1 && timeout_.getInterByteMilliseconds() == std::numeric_limits::max()) { + size_t bytes_available = available(); + if (bytes_available + total_bytes_read < size) { + waitByteTimes(size - (bytes_available + total_bytes_read)); + } + } + size_t bytes_read = ::read(fd_, buf + total_bytes_read, size - total_bytes_read); + if (bytes_read < 1) { // at least one byte is for sure available + throw SerialIOException("failure during ::read()", errno); + } + total_bytes_read += bytes_read; + } + } + return total_bytes_read; +} + +size_t Serial::SerialImpl::write(const uint8_t *data, size_t size) { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + auto write_deadline = timeout_.getWriteDeadline(size); + size_t total_bytes_written = ::write(fd_, data, size); + if (total_bytes_written == -1) { + throw SerialIOException("failure during ::write()", errno); + } + while (total_bytes_written < size) { + auto remaining_time = Timeout::remainingMicroseconds(write_deadline); + if (remaining_time.count() <= 0) { + break; + } + if (waitWritable(std::chrono::duration_cast(remaining_time))) { + size_t bytes_written = ::write(fd_, data + total_bytes_written, size - total_bytes_written); + if (bytes_written < 1) { // at least one byte is for sure available + throw SerialIOException("failure during ::write()", errno); + } + total_bytes_written += bytes_written; + } + } + return total_bytes_written; +} + +void Serial::SerialImpl::setPort(const std::string &port) { + port_ = port; +} + +std::string Serial::SerialImpl::getPort() const { + return port_; +} + +void Serial::SerialImpl::setTimeout(const Timeout &timeout) { + timeout_ = timeout; // timeout is used directly inside read() and write(): there is no need to call reconfigurePort() +} + +Serial::Timeout Serial::SerialImpl::getTimeout() const { + return timeout_; +} + +void Serial::SerialImpl::setBaudrate(unsigned long baudrate) { + baudrate_ = baudrate; + if (is_open_) { + reconfigurePort(); + } +} + +unsigned long Serial::SerialImpl::getBaudrate() const { + return baudrate_; +} + +void Serial::SerialImpl::setBytesize(serial::bytesize_t bytesize) { + bytesize_ = bytesize; + if (is_open_) { + reconfigurePort(); + } +} + +serial::bytesize_t Serial::SerialImpl::getBytesize() const { + return bytesize_; +} + +void Serial::SerialImpl::setParity(serial::parity_t parity) { + parity_ = parity; + if (is_open_) { + reconfigurePort(); + } +} + +serial::parity_t Serial::SerialImpl::getParity() const { + return parity_; +} + +void Serial::SerialImpl::setStopbits(serial::stopbits_t stopbits) { + stopbits_ = stopbits; + if (is_open_) { + reconfigurePort(); + } +} + +serial::stopbits_t Serial::SerialImpl::getStopbits() const { + return stopbits_; +} + +void Serial::SerialImpl::setFlowcontrol(serial::flowcontrol_t flowcontrol) { + flowcontrol_ = flowcontrol; + if (is_open_) { + reconfigurePort(); + } +} + +serial::flowcontrol_t Serial::SerialImpl::getFlowcontrol() const { + return flowcontrol_; +} + +void Serial::SerialImpl::flush() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (::tcflush(fd_, TCIOFLUSH) == -1) { + throw SerialIOException("failure during ::tcflush()", errno); + } +} + +void Serial::SerialImpl::flushInput() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (::tcflush(fd_, TCIFLUSH) == -1) { + throw SerialIOException("failure during ::tcflush()", errno); + } +} + +void Serial::SerialImpl::flushOutput() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (::tcflush(fd_, TCOFLUSH) == -1) { + throw SerialIOException("failure during ::tcflush()", errno); + } +} + +void Serial::SerialImpl::sendBreak(int duration_ms) const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (::tcsendbreak(fd_, duration_ms) == -1) { + throw SerialIOException("failure during ::tcsendbreak()", errno); + } +} + +void Serial::SerialImpl::setBreak(bool level) const { + setModemStatus(level ? TIOCSBRK : TIOCCBRK); +} + +void Serial::SerialImpl::setModemStatus(uint32_t request, uint32_t command) const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (::ioctl(fd_, request, &command) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } +} + +void Serial::SerialImpl::setRTS(bool level) const { + setModemStatus(level ? TIOCMBIS : TIOCMBIC, TIOCM_RTS); +} + +void Serial::SerialImpl::setDTR(bool level) const { + setModemStatus(level ? TIOCMBIS : TIOCMBIC, TIOCM_DTR); +} + +void Serial::SerialImpl::waitForModemChanges() const { +#ifndef TIOCMIWAIT + throw SerialException("TIOCMIWAIT is not defined"); +#else + if (!is_open_) { + throw SerialPortNotOpenException(); + } + // cannot use setModemStatus(): TIOCMIWAIT requires arg by value (not by pointer) + if (::ioctl(fd_, TIOCMIWAIT, TIOCM_CTS | TIOCM_DSR | TIOCM_RI | TIOCM_CD) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } +#endif +} + +uint32_t Serial::SerialImpl::getModemStatus() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + uint32_t modem_status; + if (::ioctl(fd_, TIOCMGET, &modem_status) == -1) { + throw SerialIOException("failure during ::ioctl()", errno); + } + return modem_status; +} + +bool Serial::SerialImpl::getCTS() const { + return getModemStatus() & TIOCM_CTS; +} + +bool Serial::SerialImpl::getDSR() const { + return getModemStatus() & TIOCM_DSR; +} + +bool Serial::SerialImpl::getRI() const { + return getModemStatus() & TIOCM_RI; +} + +bool Serial::SerialImpl::getCD() const { + return getModemStatus() & TIOCM_CD; +} + +#endif // !defined(_WIN32) diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_win.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_win.cpp new file mode 100644 index 0000000..935cb4b --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/impl_win.cpp @@ -0,0 +1,374 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#if defined(_WIN32) + +#include + +using namespace serial; + +Serial::SerialImpl::SerialImpl(std::string port, unsigned long baudrate, Timeout timeout, bytesize_t bytesize, + parity_t parity, stopbits_t stopbits, flowcontrol_t flowcontrol) + : port_(std::move(port)), + fd_(INVALID_HANDLE_VALUE), + is_open_(false), + timeout_(timeout), + baudrate_(baudrate), + parity_(parity), + bytesize_(bytesize), + stopbits_(stopbits), + flowcontrol_(flowcontrol) { + if (!port_.empty()) { + open(); + } +} + +Serial::SerialImpl::~SerialImpl() { + close(); +} + +void Serial::SerialImpl::open() { + if (port_.empty()) { + throw SerialInvalidArgumentException("serial port is empty."); + } + if (is_open_) { + return; + } + + std::string escaped_port = escape(port_); + fd_ = ::CreateFileW(std::wstring(escaped_port.begin(), escaped_port.end()).c_str(), GENERIC_READ | GENERIC_WRITE, 0, nullptr, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, nullptr); + if (fd_ == INVALID_HANDLE_VALUE) { + throw SerialIOException("failure during ::CreateFileW()", ::GetLastError()); + } + + reconfigurePort(); + is_open_ = true; +} + +void Serial::SerialImpl::reconfigurePort() { + if (fd_ == INVALID_HANDLE_VALUE) { + throw SerialInvalidArgumentException("invalid file descriptor"); + } + + DCB dcbSerialParams = {0}; + dcbSerialParams.DCBlength = sizeof(dcbSerialParams); + if (!::GetCommState(fd_, &dcbSerialParams)) { + throw SerialIOException("failure during ::GetCommState()", ::GetLastError()); + } + + dcbSerialParams.BaudRate = baudrate_; + + switch (bytesize_) { + case eightbits: dcbSerialParams.ByteSize = 8; break; + case sevenbits: dcbSerialParams.ByteSize = 7; break; + case sixbits: dcbSerialParams.ByteSize = 6; break; + case fivebits: dcbSerialParams.ByteSize = 5; break; + default: + throw SerialInvalidArgumentException("invalid byte size"); + } + + switch (stopbits_) { + case stopbits_one: dcbSerialParams.StopBits = ONESTOPBIT; break; + case stopbits_one_point_five: dcbSerialParams.StopBits = ONE5STOPBITS; break; + case stopbits_two: dcbSerialParams.StopBits = TWOSTOPBITS; break; + default: + throw SerialInvalidArgumentException("invalid stop bit"); + } + + switch (parity_) { + case parity_none: dcbSerialParams.Parity = NOPARITY; break; + case parity_even: dcbSerialParams.Parity = EVENPARITY; break; + case parity_odd: dcbSerialParams.Parity = ODDPARITY; break; + case parity_mark: dcbSerialParams.Parity = MARKPARITY; break; + case parity_space: dcbSerialParams.Parity = SPACEPARITY; break; + default: + throw SerialInvalidArgumentException("invalid parity"); + } + + //TODO: missing fOutxDsrFlow and fDtrControl (and many others) + switch (flowcontrol_) { + case flowcontrol_none: + dcbSerialParams.fOutxCtsFlow = false; + dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE; + dcbSerialParams.fOutX = false; + dcbSerialParams.fInX = false; + break; + case flowcontrol_software: + dcbSerialParams.fOutxCtsFlow = false; + dcbSerialParams.fRtsControl = RTS_CONTROL_DISABLE; + dcbSerialParams.fOutX = true; + dcbSerialParams.fInX = true; + break; + case flowcontrol_hardware: + dcbSerialParams.fOutxCtsFlow = true; + dcbSerialParams.fRtsControl = RTS_CONTROL_HANDSHAKE; + dcbSerialParams.fOutX = false; + dcbSerialParams.fInX = false; + break; + default: + throw SerialInvalidArgumentException("invalid flow control"); + } + + if (!::SetCommState(fd_, &dcbSerialParams)) { + throw SerialIOException("failure during ::SetCommState()", ::GetLastError()); + } + + COMMTIMEOUTS timeouts = {0}; + timeouts.ReadIntervalTimeout = timeout_.getInterByteMilliseconds(); + timeouts.ReadTotalTimeoutConstant = timeout_.getReadConstantMilliseconds(); + timeouts.ReadTotalTimeoutMultiplier = timeout_.getReadMultiplierMilliseconds(); + timeouts.WriteTotalTimeoutConstant = timeout_.getWriteConstantMilliseconds(); + timeouts.WriteTotalTimeoutMultiplier = timeout_.getWriteMultiplierMilliseconds(); + if (!::SetCommTimeouts(fd_, &timeouts)) { + throw SerialIOException("failure during ::SetCommTimeouts()", ::GetLastError()); + } +} + +void Serial::SerialImpl::close() { + if (is_open_ && fd_ != INVALID_HANDLE_VALUE && !::CloseHandle(fd_)) { + throw SerialIOException("failure during ::CloseHandle()", ::GetLastError()); + } + fd_ = INVALID_HANDLE_VALUE; + is_open_ = false; +} + +bool Serial::SerialImpl::isOpen() const { + return is_open_; +} + +size_t Serial::SerialImpl::available() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + COMSTAT stat; + if (!::ClearCommError(fd_, nullptr, &stat)) { + throw SerialIOException("during ::ClearCommError()", ::GetLastError()); + } + return static_cast(stat.cbInQue); +} + +bool Serial::SerialImpl::waitReadable(std::chrono::milliseconds timeout_ms) const { + throw SerialException("waitReadable() is not implemented on Windows"); + //TODO: have a deeper look at WaitForSingleObject +} + +bool Serial::SerialImpl::waitWritable(std::chrono::milliseconds timeout_ms) const { + throw SerialException("waitWritable() is not implemented on Windows"); + //TODO: have a deeper look at WaitForSingleObject +} + +void Serial::SerialImpl::waitByteTimes(size_t count) const { + auto start = std::chrono::steady_clock::now(); + uint32_t bit_time_ns = 1e9 / baudrate_; + uint32_t byte_time_ns = bit_time_ns * (1 + bytesize_ + parity_ + stopbits_); + byte_time_ns += stopbits_ == stopbits_one_point_five ? -1.5*bit_time_ns : 0; // stopbits_one_point_five is 3 + std::this_thread::sleep_until(start + std::chrono::nanoseconds(byte_time_ns * count)); +} + +size_t Serial::SerialImpl::read(uint8_t *buf, size_t size) { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + DWORD bytes_read = 0; + if (!::ReadFile(fd_, buf, static_cast(size), &bytes_read, nullptr)) { + throw SerialIOException("during ::ReadFile()", ::GetLastError()); + } + return static_cast(bytes_read); +} + +size_t Serial::SerialImpl::write(const uint8_t *data, size_t size) { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + DWORD bytes_written = 0; + if (!::WriteFile(fd_, data, static_cast(size), &bytes_written, nullptr)) { + throw SerialIOException("during ::WriteFile()", ::GetLastError()); + } + return static_cast(bytes_written); +} + +void Serial::SerialImpl::setPort(const std::string &port) { + port_ = port; +} + +std::string Serial::SerialImpl::getPort() const { + return port_; +} + +void Serial::SerialImpl::setTimeout(const Timeout &timeout) { + timeout_ = timeout; + if (is_open_) { + reconfigurePort(); + } +} + +Serial::Timeout Serial::SerialImpl::getTimeout() const { + return timeout_; +} + +void Serial::SerialImpl::setBaudrate(unsigned long baudrate) { + baudrate_ = baudrate; + if (is_open_) { + reconfigurePort(); + } +} + +unsigned long Serial::SerialImpl::getBaudrate() const { + return baudrate_; +} + +void Serial::SerialImpl::setBytesize(serial::bytesize_t bytesize) { + bytesize_ = bytesize; + if (is_open_) { + reconfigurePort(); + } +} + +serial::bytesize_t Serial::SerialImpl::getBytesize() const { + return bytesize_; +} + +void Serial::SerialImpl::setParity(serial::parity_t parity) { + parity_ = parity; + if (is_open_) { + reconfigurePort(); + } +} + +serial::parity_t Serial::SerialImpl::getParity() const { + return parity_; +} + +void Serial::SerialImpl::setStopbits(serial::stopbits_t stopbits) { + stopbits_ = stopbits; + if (is_open_) { + reconfigurePort(); + } +} + +serial::stopbits_t Serial::SerialImpl::getStopbits() const { + return stopbits_; +} + +void Serial::SerialImpl::setFlowcontrol(serial::flowcontrol_t flowcontrol) { + flowcontrol_ = flowcontrol; + if (is_open_) { + reconfigurePort(); + } +} + +serial::flowcontrol_t Serial::SerialImpl::getFlowcontrol() const { + return flowcontrol_; +} + +void Serial::SerialImpl::flush() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (!::PurgeComm(fd_, PURGE_RXCLEAR | PURGE_TXCLEAR)) { + throw SerialIOException("failure during ::EscapeCommFunction()"); + } +} + +void Serial::SerialImpl::flushInput() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (!::PurgeComm(fd_, PURGE_RXCLEAR)) { + throw SerialIOException("failure during ::EscapeCommFunction()"); + } +} + +void Serial::SerialImpl::flushOutput() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (!::PurgeComm(fd_, PURGE_TXCLEAR)) { + throw SerialIOException("failure during ::EscapeCommFunction()"); + } +} + +void Serial::SerialImpl::sendBreak(int duration_ms) const { + throw SerialException("sendBreak() is not implemented on Windows"); +} + +void Serial::SerialImpl::setBreak(bool level) const { + setModemStatus(level ? SETBREAK : CLRBREAK); +} + +void Serial::SerialImpl::setModemStatus(uint32_t request, uint32_t command) const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (!::EscapeCommFunction(fd_, request)) { + throw SerialIOException("failure during ::EscapeCommFunction()"); + } +} + +void Serial::SerialImpl::setRTS(bool level) const { + setModemStatus(level ? SETRTS : CLRRTS); +} + +void Serial::SerialImpl::setDTR(bool level) const { + setModemStatus(level ? SETDTR : CLRDTR); +} + +void Serial::SerialImpl::waitForModemChanges() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + if (!::SetCommMask(fd_, EV_CTS | EV_DSR | EV_RING | EV_RLSD)) { + throw SerialIOException("failure during ::SetCommMask()"); + } + DWORD event; + if (!::WaitCommEvent(fd_, &event, nullptr)) { + throw SerialIOException("failure during ::WaitCommEvent()"); + } +} + +uint32_t Serial::SerialImpl::getModemStatus() const { + if (!is_open_) { + throw SerialPortNotOpenException(); + } + DWORD modem_status; + if (!::GetCommModemStatus(fd_, &modem_status)) { + throw SerialIOException("failure during ::GetCommModemStatus()"); + } + return modem_status; +} + +bool Serial::SerialImpl::getCTS() const { + return getModemStatus() & MS_CTS_ON; +} + +bool Serial::SerialImpl::getDSR() const { + return getModemStatus() & MS_DSR_ON; +} + +bool Serial::SerialImpl::getRI() const { + return getModemStatus() & MS_RING_ON; +} + +bool Serial::SerialImpl::getCD() const { + return getModemStatus() & MS_RLSD_ON; +} + +#endif // defined(_WIN32) diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_linux.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_linux.cpp new file mode 100644 index 0000000..a3d9d12 --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_linux.cpp @@ -0,0 +1,151 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2014 Craig Lilley + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#if defined(__linux__) + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include + +using namespace serial; + +int getLinkPath(std::string system_path, std::string &link_path) { + struct stat serial_port_stat{0}; + if (::lstat(system_path.c_str(), &serial_port_stat) == -1) { + return -1; + } + if (!S_ISLNK(serial_port_stat.st_mode)) { + system_path += "/device"; + } + char link_path_buf[PATH_MAX]; + ssize_t link_length = ::readlink(system_path.c_str(), link_path_buf, sizeof(link_path_buf) - 1); + if (link_length == -1) { + return -1; + } + link_path_buf[link_length] = '\0'; + link_path = std::string(link_path_buf); + return 0; +} + +int PortInfo::getPortInfo(const std::string &serial_port_name) { + std::smatch serial_port_match; + std::regex_match(serial_port_name, serial_port_match, std::regex("^/dev/([^/]+)/?$")); + if (serial_port_match.size() != 2) { + //TODO: wrong serial port name + return -1; + } + std::string serial_port_substr(serial_port_match[1]); + + std::string link_path; + std::string system_path("/sys/class/tty/" + serial_port_substr); + if (getLinkPath(system_path, link_path)) { + //TODO: device not found in sys or link read error + return -1; + } + + if (std::strstr(link_path.c_str(), "usb")) { + system_path = "/sys/class/tty/" + serial_port_substr + "/device"; + for (int i=0; i<3; i++) { + system_path += "/.."; + //FIXME: should check the existence at least for the first four files + struct stat serial_port_stat{0}; + if (::stat((system_path + "/busnum").c_str(), &serial_port_stat) == -1) { + continue; + } + std::ifstream(system_path + "/busnum") >> busnum; + std::ifstream(system_path + "/devnum") >> devnum; + std::ifstream(system_path + "/idProduct") >> std::hex >> id_product; + std::ifstream(system_path + "/idVendor") >> std::hex >> id_vendor; + std::getline(std::ifstream(system_path + "/manufacturer"), manufacturer); + std::getline(std::ifstream(system_path + "/product"), product); + std::getline(std::ifstream(system_path + "/serial"), serial_number); + break; + } + } + serial_port = serial_port_name; + return 0; +} + +int serial::getPortsInfo(std::vector &serial_ports) { + serial_ports.clear(); + std::vector serial_port_names; + if (getPortsList(serial_port_names) < 0) { + return -1; + } + for (auto const &serial_port_name : serial_port_names) { + PortInfo serial_port; + if (!serial_port.getPortInfo(serial_port_name)) { + serial_ports.push_back(serial_port); + } + } + return serial_ports.size(); +} + +int serial::getPortsList(std::vector &serial_port_names) { + serial_port_names.clear(); + + glob_t glob_results; + std::string glob_pattern("/sys/class/tty/*"); + if (::glob(glob_pattern.c_str(), 0, nullptr, &glob_results)) { + // 'serial_port_names' is cleared + globfree(&glob_results); + return -1; + } + + for (int i=0; i +#include +#include + +#include + +using namespace serial; + +int toInt(CFNumberRef cf_num) { + int num; + if (cf_num && ::CFNumberGetValue(cf_num, kCFNumberIntType, &num)) { + return num; + } + return 0; +} + +std::string toString(CFStringRef cf_str) { + char str[PATH_MAX] = {}; + if (cf_str && ::CFStringGetCString(cf_str, str, sizeof(str), kCFStringEncodingASCII)) { + return std::string(str); + } + return ""; +} + +std::string getDevicePath(const io_object_t &serial_port_obj) { + CFTypeRef path = ::IORegistryEntryCreateCFProperty(serial_port_obj, CFSTR(kIOCalloutDeviceKey), kCFAllocatorDefault, 0); + std::string device_path = toString(reinterpret_cast(path)); + if (path) { + ::CFRelease(path); + } + return device_path; +} + +std::string getPortPropertyString(const io_object_t &serial_port_obj, const std::string &property_name) { + CFStringRef cf_property_name = ::CFStringCreateWithCString(kCFAllocatorDefault, property_name.c_str(), kCFStringEncodingASCII); + CFTypeRef cf_property = ::IORegistryEntrySearchCFProperty(serial_port_obj, kIOServicePlane, cf_property_name, kCFAllocatorDefault, kIORegistryIterateRecursively | kIORegistryIterateParents); + std::string property_value = toString(reinterpret_cast(cf_property)); + if (cf_property) { + ::CFRelease(cf_property); + } + return property_value; +} + +uint16_t getPortPropertyValue(const io_object_t &serial_port_obj, const std::string &property_name) { + CFStringRef cf_property_name = ::CFStringCreateWithCString(kCFAllocatorDefault, property_name.c_str(), kCFStringEncodingASCII); + CFTypeRef cf_property = ::IORegistryEntrySearchCFProperty(serial_port_obj, kIOServicePlane, cf_property_name, kCFAllocatorDefault, kIORegistryIterateRecursively | kIORegistryIterateParents); + int property_value = toInt(reinterpret_cast(cf_property)); + if (cf_property) { + ::CFRelease(cf_property); + } + return property_value; +} + +std::string getClassName(const io_object_t &device_obj) { + io_name_t class_name; + return ::IOObjectGetClass(device_obj, class_name) == KERN_SUCCESS ? class_name : ""; +} + +io_registry_entry_t getParent(io_object_t &serial_port_obj) { + io_object_t parent = serial_port_obj; + while (getClassName(parent) != "IOUSBDevice") { + if (::IORegistryEntryGetParentEntry(parent, kIOServicePlane, &parent) != KERN_SUCCESS) { + return 0; + } + } + return parent; +} + +int PortInfo::getPortInfo(const std::string &serial_port_name) { + CFMutableDictionaryRef classes; + if (!(classes = ::IOServiceMatching(kIOSerialBSDServiceValue))) { + return -1; + } + + io_iterator_t serial_port_iterator; + if (::IOServiceGetMatchingServices(kIOMasterPortDefault, classes, &serial_port_iterator) != KERN_SUCCESS) { + return -1; + } + + io_object_t serial_port_obj; + while ((serial_port_obj = ::IOIteratorNext(serial_port_iterator))) { + std::string serial_port_name_found = getDevicePath(serial_port_obj); + if (serial_port_name_found != serial_port_name) { + continue; + } + serial_port = serial_port_name_found; + + io_registry_entry_t parent = getParent(serial_port_obj); + if (parent == 0) { + continue; + } + ::IOObjectRelease(parent); + + devnum = getPortPropertyValue(serial_port_obj, "USB Address"); + id_product = getPortPropertyValue(serial_port_obj, "idProduct"); + id_vendor = getPortPropertyValue(serial_port_obj, "idVendor"); + manufacturer = getPortPropertyString(serial_port_obj, "USB Vendor Name"); + product = getPortPropertyString(serial_port_obj, "USB Product Name"); + serial_number = getPortPropertyString(serial_port_obj, "USB Serial Number"); + ::IOObjectRelease(serial_port_obj); + } + + ::IOObjectRelease(serial_port_iterator); + return serial_port != serial_port_name ? -1 : 0; +} + +int serial::getPortsInfo(std::vector &serial_ports) { + serial_ports.clear(); + std::vector serial_port_names; + if (getPortsList(serial_port_names) < 0) { + return -1; + } + for (auto const &serial_port_name : serial_port_names) { + PortInfo serial_port; + if (!serial_port.getPortInfo(serial_port_name)) { + serial_ports.push_back(serial_port); + } + } + return serial_ports.size(); +} + +int serial::getPortsList(std::vector &serial_port_names) { + serial_port_names.clear(); + + CFMutableDictionaryRef classes; + if (!(classes = ::IOServiceMatching(kIOSerialBSDServiceValue))) { + return -1; + } + + io_iterator_t serial_port_iterator; + if (::IOServiceGetMatchingServices(kIOMasterPortDefault, classes, &serial_port_iterator) != KERN_SUCCESS) { + return -1; + } + + io_object_t serial_port_obj; + while ((serial_port_obj = ::IOIteratorNext(serial_port_iterator))) { + serial_port_names.push_back(getDevicePath(serial_port_obj)); + ::IOObjectRelease(serial_port_obj); + } + + ::IOObjectRelease(serial_port_iterator); + return serial_port_names.size(); +} + +#endif // defined(__APPLE__) diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_win.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_win.cpp new file mode 100644 index 0000000..9806e6a --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/impl/list_ports/list_ports_win.cpp @@ -0,0 +1,307 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2014 Craig Lilley + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#if defined(_WIN32) + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace serial; + +template +std::shared_ptr sharedPtr(size_t size = 0) { + std::shared_ptr ptr(static_cast(::malloc(sizeof(T) + size)), ::free); + ::memset(ptr.get(), 0, sizeof(T)+size); + return ptr; +} + +std::wstring toWString(const std::string& str) { + return std::wstring_convert>().from_bytes(str); +} + +std::string toString(const std::wstring& wstr) { + return std::wstring_convert>().to_bytes(wstr); +} + +std::string getDeviceDescriptor(HANDLE handle, DWORD connection_index, UCHAR descriptor_index) { + if (!descriptor_index) { + return ""; + } + + char buffer[sizeof(USB_DESCRIPTOR_REQUEST) + MAXIMUM_USB_STRING_LENGTH] = {0}; + auto request = reinterpret_cast(buffer); + auto descriptor = reinterpret_cast(request+1); + DWORD size = sizeof(buffer); + + request->ConnectionIndex = connection_index; + request->SetupPacket.bmRequest = 0x80; + request->SetupPacket.bRequest = USB_REQUEST_GET_DESCRIPTOR; + request->SetupPacket.wIndex = 0; + request->SetupPacket.wLength = MAXIMUM_USB_STRING_LENGTH; + request->SetupPacket.wValue = (USB_STRING_DESCRIPTOR_TYPE << 8) | descriptor_index; + if (!::DeviceIoControl(handle, IOCTL_USB_GET_DESCRIPTOR_FROM_NODE_CONNECTION, request, size, request, size, &size, nullptr)) { + return ""; + } + + return toString(descriptor->bString); +} + +std::string getDevicePath(const GUID &guid, DEVINST instance, DWORD &busnum) { + std::string device_path; + HDEVINFO device_info = ::SetupDiGetClassDevsA(&guid, nullptr, nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + if (device_info != INVALID_HANDLE_VALUE) { + auto device_info_data = sharedPtr(); + device_info_data->cbSize = sizeof(SP_DEVINFO_DATA); + for (int i=0; ::SetupDiEnumDeviceInfo(device_info, i, device_info_data.get()); i++) { + DEVINST instance_actual = instance; + auto device_interface_data = sharedPtr(); + device_interface_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DATA); + for (int j=0; ::SetupDiEnumDeviceInterfaces(device_info, device_info_data.get(), &guid, j, device_interface_data.get()); j++) { + DWORD required_size = 0; + if (!::SetupDiGetDeviceInterfaceDetailA(device_info, device_interface_data.get(), nullptr, 0, &required_size, nullptr) && GetLastError() != ERROR_INSUFFICIENT_BUFFER) { + continue; + } + + auto device_detail_data = sharedPtr(required_size); + device_detail_data->cbSize = sizeof(SP_DEVICE_INTERFACE_DETAIL_DATA_A); + if (!::SetupDiGetDeviceInterfaceDetailA(device_info, device_interface_data.get(), device_detail_data.get(), required_size, nullptr, device_info_data.get())) { + continue; + } + + while (::CM_Get_Parent(&instance_actual, instance_actual, 0) == CR_SUCCESS && instance_actual != device_info_data->DevInst) { + ; + } + if (instance_actual == device_info_data->DevInst) { + busnum = i + 1; + device_path = std::string(device_detail_data->DevicePath); + break; + } + } + if (!device_path.empty()) { + break; + } + } + } + ::SetupDiDestroyDeviceInfoList(device_info); + return device_path; +} + +std::string getHubName(HANDLE handle, DWORD connection_index) { + DWORD size = sizeof(USB_NODE_CONNECTION_NAME); + auto hub_name_tmp = sharedPtr(); + hub_name_tmp->ConnectionIndex = connection_index; + if (!::DeviceIoControl(handle, IOCTL_USB_GET_NODE_CONNECTION_NAME, hub_name_tmp.get(), size, hub_name_tmp.get(), size, &size, nullptr)) { + return ""; + } + + size = hub_name_tmp->ActualLength; + auto hub_name = sharedPtr(size); + hub_name->ConnectionIndex = connection_index; + if (!::DeviceIoControl(handle, IOCTL_USB_GET_NODE_CONNECTION_NAME, hub_name.get(), size, hub_name.get(), size, &size, nullptr)) { + return ""; + } + + return toString(hub_name->NodeName); +} + +std::string getRootName(HANDLE handle) { + DWORD size = sizeof(USB_ROOT_HUB_NAME); + auto root_name_tmp = sharedPtr(); + if (!::DeviceIoControl(handle, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, root_name_tmp.get(), size, &size, nullptr)) { + return ""; + } + + size = root_name_tmp->ActualLength; + auto root_name = sharedPtr(size); + if (!::DeviceIoControl(handle, IOCTL_USB_GET_ROOT_HUB_NAME, nullptr, 0, root_name.get(), size, &size, nullptr)) { + return ""; + } + + return toString(root_name->RootHubName); +} + +std::string getPortNameFromFriendlyName(const std::string& friendly_name) { + std::smatch serial_port_match; + std::regex_match(friendly_name, serial_port_match, std::regex("^.*(COM\\d+).*$")); + if (serial_port_match.size() != 2) { + return ""; + } + return std::string(serial_port_match[1]); +} + +DWORD getPortPropertyValue(HDEVINFO &device_info_set, SP_DEVINFO_DATA &device_info_data, DEVPROPKEY property) { + DEVPROPTYPE devPropType; + DWORD length = 0; + DWORD buffer = 0; + if (!::SetupDiGetDeviceProperty(device_info_set, &device_info_data, &property, &devPropType, reinterpret_cast(&buffer), sizeof(buffer), &length, 0)) { + return 0; + } + return buffer; +} + +std::string getPortPropertyString(HDEVINFO &device_info_set, SP_DEVINFO_DATA &device_info_data, DEVPROPKEY property) { + DEVPROPTYPE devPropType; + DWORD length = 0; + WCHAR buffer[256] = {0}; + if (!::SetupDiGetDeviceProperty(device_info_set, &device_info_data, &property, &devPropType, reinterpret_cast(buffer), sizeof(buffer), &length, 0)) { + return ""; + } + return toString(buffer); +} + +int getCOMPorts(std::map &serial_port_numbers, std::map &serial_port_instances) { + serial_port_numbers.clear(); + serial_port_instances.clear(); + HDEVINFO device_info = ::SetupDiGetClassDevsA(&GUID_DEVINTERFACE_COMPORT, nullptr, nullptr, DIGCF_PRESENT | DIGCF_DEVICEINTERFACE); + if (device_info == INVALID_HANDLE_VALUE) { + return -1; + } + + SP_DEVINFO_DATA device_info_data {.cbSize = sizeof(SP_DEVINFO_DATA)}; + for (int i=0; ::SetupDiEnumDeviceInfo(device_info, i, &device_info_data); i++) { + DWORD port_number = getPortPropertyValue(device_info, device_info_data, DEVPKEY_Device_Address); + std::string hardware_id = getPortPropertyString(device_info, device_info_data, DEVPKEY_Device_HardwareIds); // only for debug + std::string friendly_name = getPortPropertyString(device_info, device_info_data, DEVPKEY_Device_FriendlyName); + std::string serial_port_name = getPortNameFromFriendlyName(friendly_name); + + serial_port_numbers.insert(std::make_pair(serial_port_name, port_number)); + serial_port_instances.insert(std::make_pair(serial_port_name, device_info_data.DevInst)); + } + + ::SetupDiDestroyDeviceInfoList(device_info); + return serial_port_numbers.size(); +} + +int getInfo(HANDLE handle, DWORD serial_port_number, HANDLE &handle_found, std::shared_ptr &connection_info_found) { + USB_NODE_INFORMATION node_info{}; + DWORD size = sizeof(node_info); + if (::DeviceIoControl(handle, IOCTL_USB_GET_NODE_INFORMATION, &node_info, size, &node_info, size, &size, nullptr)) { + for (int port_number=1; port_number <= node_info.u.HubInformation.HubDescriptor.bNumberOfPorts; port_number++) { + auto connection_info = sharedPtr(); + connection_info->ConnectionIndex = port_number; + size = sizeof(*connection_info); + if (!::DeviceIoControl(handle, IOCTL_USB_GET_NODE_CONNECTION_INFORMATION_EX, connection_info.get(), size, connection_info.get(), size, &size, nullptr)) { + continue; + } + if (connection_info->DeviceIsHub) { + HANDLE hub_handle = ::CreateFileA(escape(getHubName(handle, connection_info->ConnectionIndex)).c_str(), GENERIC_WRITE, FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr); + if (hub_handle == INVALID_HANDLE_VALUE) { + continue; + } + if (getInfo(hub_handle, serial_port_number, handle_found, connection_info_found)) { + ::CloseHandle(hub_handle); + continue; + } + return 0; + } + if (connection_info->ConnectionIndex != serial_port_number) { + continue; + } + + handle_found = handle; + connection_info_found = connection_info; + return 0; + } + } + handle_found = INVALID_HANDLE_VALUE; + connection_info_found = nullptr; + return -1; +} + +int PortInfo::getPortInfo(const std::string &serial_port_name) { + std::map serial_port_numbers; + std::map serial_port_instances; + getCOMPorts(serial_port_numbers, serial_port_instances); + if (!serial_port_numbers.count(serial_port_name)) { + return -1; + } + + DWORD bus_number = 0; + std::string device_path = getDevicePath(GUID_DEVINTERFACE_USB_HOST_CONTROLLER, serial_port_instances.at(serial_port_name), bus_number); + HANDLE device_handle = ::CreateFileA(device_path.c_str(), GENERIC_WRITE, FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr); + if (device_handle == INVALID_HANDLE_VALUE) { + return -1; + } + HANDLE root_handle = ::CreateFileA(escape(getRootName(device_handle)).c_str(), GENERIC_WRITE, FILE_SHARE_WRITE, nullptr, OPEN_EXISTING, 0, nullptr); + if (root_handle == INVALID_HANDLE_VALUE) { + ::CloseHandle(device_handle); + return -1; + } + + HANDLE handle_found = INVALID_HANDLE_VALUE; + auto connection_info_found = sharedPtr(); + if (!getInfo(root_handle, serial_port_numbers.at(serial_port_name), handle_found, connection_info_found)) { + busnum = bus_number; + devnum = connection_info_found->DeviceAddress + 1; + id_vendor = connection_info_found->DeviceDescriptor.idVendor; + id_product = connection_info_found->DeviceDescriptor.idProduct; + manufacturer = getDeviceDescriptor(handle_found, connection_info_found->ConnectionIndex, connection_info_found->DeviceDescriptor.iManufacturer); + product = getDeviceDescriptor(handle_found, connection_info_found->ConnectionIndex, connection_info_found->DeviceDescriptor.iProduct); + serial_number = getDeviceDescriptor(handle_found, connection_info_found->ConnectionIndex, connection_info_found->DeviceDescriptor.iSerialNumber); + ::CloseHandle(handle_found); + } + + if (handle_found != root_handle) { + ::CloseHandle(root_handle); + } + ::CloseHandle(device_handle); + serial_port = serial_port_name; + return 0; +} + +int serial::getPortsInfo(std::vector &serial_ports) { + serial_ports.clear(); + std::vector serial_port_names; + if (getPortsList(serial_port_names) < 0) { + return -1; + } + for (auto const &serial_port_name : serial_port_names) { + PortInfo serial_port; + if (!serial_port.getPortInfo(serial_port_name)) { + serial_ports.push_back(serial_port); + } + } + return serial_ports.size(); +} + +int serial::getPortsList(std::vector &serial_port_names) { + serial_port_names.clear(); + std::map serial_port_numbers; + std::map serial_port_instances; + getCOMPorts(serial_port_numbers, serial_port_instances); + for (auto const& serial_port : serial_port_numbers) { + serial_port_names.push_back(serial_port.first); + } + return serial_port_names.size(); +} + +#endif // #if defined(_WIN32) diff --git a/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/serial.cpp b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/serial.cpp new file mode 100644 index 0000000..60e3eca --- /dev/null +++ b/qbdevice-ros/qb_device_driver/api/qbdevice-api-7.x.x/serial/src/serial.cpp @@ -0,0 +1,268 @@ +/*** + * MIT License + * + * Copyright (c) 2020 Alessandro Tondo + * Copyright (c) 2012 William Woodall, John Harrison + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated + * documentation files (the "Software"), to deal in the Software without restriction, including without limitation + * the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and + * to permit persons to whom the Software is furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all copies or substantial portions of + * the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO + * THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, + * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +using namespace serial; + +Serial::Serial(const std::string &port_name, uint32_t baudrate, Serial::Timeout timeout, bytesize_t bytesize, parity_t parity, + stopbits_t stopbits, flowcontrol_t flowcontrol) + : pimpl_(new SerialImpl(port_name, baudrate, timeout, bytesize, parity, stopbits, flowcontrol)) { + +} + +Serial::~Serial() { +// pimpl_ is automatically destroyed and close() is called inside pimpl_ destructor +} + +void Serial::open() { + pimpl_->open(); +} + +void Serial::close() { + pimpl_->close(); +} + +bool Serial::isOpen() const { + return pimpl_->isOpen(); +} + +size_t Serial::available() { + return pimpl_->available(); +} + +bool Serial::waitReadable() { + return pimpl_->waitReadable(pimpl_->getTimeout().getReadConstant()); +} + +bool Serial::waitWritable() { + return pimpl_->waitWritable(pimpl_->getTimeout().getWriteConstant()); +} + +void Serial::waitByteTimes(size_t count) { + pimpl_->waitByteTimes(count); +} + +size_t Serial::read(uint8_t *buffer, size_t size) { + std::lock_guard read_lock(read_mutex_); + return pimpl_->read(buffer, size); +} + +size_t Serial::read(std::vector &buffer, size_t size) { + std::lock_guard read_lock(read_mutex_); + std::unique_ptr buf(new uint8_t[size]); + size_t bytes_read = pimpl_->read(buf.get(), size); + buffer.insert(buffer.end(), buf.get(), buf.get() + bytes_read); + return bytes_read; +} + +size_t Serial::read(std::string &buffer, size_t size) { + std::lock_guard read_lock(read_mutex_); + std::unique_ptr buf(new uint8_t[size]); + size_t bytes_read = pimpl_->read(buf.get(), size); + buffer.append(reinterpret_cast(buf.get()), bytes_read); + return bytes_read; +} + +std::string Serial::read(size_t size) { + std::string buffer; + read(buffer, size); + return buffer; +} + +size_t Serial::readline_(std::string &line, size_t size, const std::string &eol) { + std::unique_ptr buffer(new uint8_t[size]); + size_t eol_len = eol.length(); + size_t read_so_far = 0; + while (read_so_far < size) { + size_t bytes_read = pimpl_->read(buffer.get() + read_so_far, 1); + read_so_far += bytes_read; + if (bytes_read == 0) { + break; // Timeout occurred on reading 1 byte + } + if (read_so_far >= eol_len && std::string(reinterpret_cast(buffer.get() + read_so_far - eol_len), eol_len) == eol) { + break; // EOL found + } + } + line.append(reinterpret_cast(buffer.get()), read_so_far); + return read_so_far; +} + +size_t Serial::readline(std::string &line, size_t size, const std::string &eol) { + std::lock_guard read_lock(read_mutex_); + return readline_(line, size, eol); +} + +std::string Serial::readline(size_t size, const std::string &eol) { + std::lock_guard read_lock(read_mutex_); + std::string line; + readline_(line, size, eol); + return line; +} + +std::vector Serial::readlines(size_t size, const std::string &eol) { + std::lock_guard read_lock(read_mutex_); + std::vector lines; + std::string line; + while (readline_(line, size, eol) > 0) { + lines.push_back(line); + size -= line.size(); + line.clear(); + } + return lines; +} + +size_t Serial::write(const std::string &data) { + std::lock_guard write_lock(write_mutex_); + return pimpl_->write(reinterpret_cast(data.c_str()), data.length()); +} + +size_t Serial::write(const std::vector &data) { + std::lock_guard write_lock(write_mutex_); + return pimpl_->write(&data[0], data.size()); +} + +size_t Serial::write(const uint8_t *data, size_t size) { + std::lock_guard write_lock(write_mutex_); + return pimpl_->write(data, size); +} + +void Serial::setPort(const std::string &port_name) { + std::lock_guard read_lock(read_mutex_); + std::lock_guard write_lock(write_mutex_); + bool was_open = pimpl_->isOpen(); + if (was_open) { + close(); + } + pimpl_->setPort(port_name); + if (was_open) { + open(); + } +} + +std::string Serial::getPort() const { + return pimpl_->getPort(); +} + +void Serial::setTimeout(const Timeout &timeout) { + pimpl_->setTimeout(timeout); +} + +void Serial::setTimeout(uint32_t inter_byte, uint32_t read_constant, uint32_t read_multiplier, uint32_t write_constant, uint32_t write_multiplier) { + setTimeout(Timeout(inter_byte, read_constant, read_multiplier, write_constant, write_multiplier)); +} + +Serial::Timeout Serial::getTimeout() const { + return pimpl_->getTimeout(); +} + +void Serial::setBaudrate(uint32_t baudrate) { + pimpl_->setBaudrate(baudrate); +} + +uint32_t Serial::getBaudrate() const { + return uint32_t(pimpl_->getBaudrate()); +} + +void Serial::setBytesize(bytesize_t bytesize) { + pimpl_->setBytesize(bytesize); +} + +bytesize_t Serial::getBytesize() const { + return pimpl_->getBytesize(); +} + +void Serial::setParity(parity_t parity) { + pimpl_->setParity(parity); +} + +parity_t Serial::getParity() const { + return pimpl_->getParity(); +} + +void Serial::setStopbits(stopbits_t stopbits) { + pimpl_->setStopbits(stopbits); +} + +stopbits_t Serial::getStopbits() const { + return pimpl_->getStopbits(); +} + +void Serial::setFlowcontrol(flowcontrol_t flowcontrol) { + pimpl_->setFlowcontrol(flowcontrol); +} + +flowcontrol_t Serial::getFlowcontrol() const { + return pimpl_->getFlowcontrol(); +} + +void Serial::flush() { + std::lock_guard read_lock(read_mutex_); + std::lock_guard write_lock(write_mutex_); + pimpl_->flush(); +} + +void Serial::flushInput() { + std::lock_guard read_lock(read_mutex_); + pimpl_->flushInput(); +} + +void Serial::flushOutput() { + std::lock_guard write_lock(write_mutex_); + pimpl_->flushOutput(); +} + +void Serial::sendBreak(int duration) { + pimpl_->sendBreak(duration); +} + +void Serial::setBreak(bool level) { + pimpl_->setBreak(level); +} + +void Serial::setRTS(bool level) { + pimpl_->setRTS(level); +} + +void Serial::setDTR(bool level) { + pimpl_->setDTR(level); +} + +void Serial::waitForModemChanges() { + pimpl_->waitForModemChanges(); +} + +bool Serial::getCTS() { + return pimpl_->getCTS(); +} + +bool Serial::getDSR() { + return pimpl_->getDSR(); +} + +bool Serial::getRI() { + return pimpl_->getRI(); +} + +bool Serial::getCD() { + return pimpl_->getCD(); +}