diff --git a/README.md b/README.md index a44906a..dcf70c2 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,11 @@ Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py` -> необходимо `warn` заменить на `warning` в логере \ No newline at end of file +> необходимо `warn` заменить на `warning` в логере + +```bash + +sudo apt install -y ros-${ROS_DISTRO}-webots-ros2 \ + ros-${ROS_DISTRO}-ros2-control \ + ros-${ROS_DISTRO}-ros2-controllers \ + ros-${ROS_DISTRO}-moveit-* \ +``` \ No newline at end of file diff --git a/src/iiwa_controller/CMakeLists.txt b/src/iiwa_controller/CMakeLists.txt new file mode 100644 index 0000000..6204799 --- /dev/null +++ b/src/iiwa_controller/CMakeLists.txt @@ -0,0 +1,104 @@ +cmake_minimum_required(VERSION 3.8) +project(iiwa_controller) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(Eigen3 REQUIRED) + +# FRI headers / sources +set(FRI_HEADER + external/libFRI/include + external/libFRI/src/protobuf_gen + external/libFRI/src/nanopb-0.2.8 + external/libFRI/src/protobuf + external/libFRI/src/connection + external/libFRI/src/client_lbr + external/libFRI/src/base + ) + +set(FRI_SRC + external/libFRI/src/base/friClientApplication.cpp + external/libFRI/src/client_lbr/friLBRClient.cpp + external/libFRI/src/client_lbr/friLBRCommand.cpp + external/libFRI/src/client_lbr/friLBRState.cpp + external/libFRI/src/connection/friUdpConnection.cpp + external/libFRI/src/protobuf/friCommandMessageEncoder.cpp + external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp + external/libFRI/src/protobuf/pb_frimessages_callbacks.c + external/libFRI/src/protobuf_gen/FRIMessages.pb.c + external/libFRI/src/nanopb-0.2.8/pb_decode.c + external/libFRI/src/nanopb-0.2.8/pb_encode.c + external/libFRI/src/client_trafo/friTransformationClient.cpp + ) + +add_library(${PROJECT_NAME} + SHARED + src/IIWAHardwareInterface.cpp + ${FRI_SRC} + src/FRIClient.cpp +) + +target_include_directories(${PROJECT_NAME} + PRIVATE + include + ${FRI_HEADER} +) + +target_compile_definitions(${PROJECT_NAME} + PRIVATE + PB_FIELD_16BIT + HAVE_SOCKLEN_T + PB_FIELD_16BIT + PB_NO_ERRMSG +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC + hardware_interface::hardware_interface + pluginlib::pluginlib + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + Eigen3::Eigen +) + +# Export plugin description for pluginlib +pluginlib_export_plugin_description_file(hardware_interface iiwa_controller_plugin.xml) + +# Installation +install(TARGETS ${PROJECT_NAME} + DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) + +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + Eigen3 +) + +ament_package() diff --git a/src/iiwa_controller/LICENSE b/src/iiwa_controller/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/iiwa_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/iiwa_controller/external/libFRI/include/friClientApplication.h b/src/iiwa_controller/external/libFRI/include/friClientApplication.h new file mode 100644 index 0000000..69bc4cc --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friClientApplication.h @@ -0,0 +1,155 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ + +#ifndef _KUKA_FRI_CLIENT_APPLICATION_H +#define _KUKA_FRI_CLIENT_APPLICATION_H + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + class IClient; + class TransformationClient; + class IConnection; + struct ClientData; + + /** + * \brief FRI client application class. + * + * A client application takes an instance of the IConnection interface and + * an instance of an IClient interface to provide the functionality + * needed to set up an FRI client application. It can be used to easily + * integrate the FRI client code within other applications. + * The algorithmic functionality of an FRI client application is implemented + * using the IClient interface. + */ + class ClientApplication + { + + public: + + /** + * \brief Constructor without transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface as parameters. + * @param connection FRI connection class + * @param client FRI client class + */ + ClientApplication(IConnection& connection, IClient& client); + + /** + * \brief Constructor with transformation client. + * + * This constructor takes an instance of the IConnection interface and + * an instance of the IClient interface and an instance of a + * TransformationClient as parameters. + * @param connection FRI connection class + * @param client FRI client class + * @param trafoClient FRI transformation client class + */ + ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient); + + /** \brief Destructor. */ + ~ClientApplication(); + + /** + * \brief Connect the FRI client application with a KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + bool connect(int port, const char *remoteHost = NULL); + + /** + * \brief Disconnect the FRI client application from a KUKA Sunrise controller. + */ + void disconnect(); + + /** + * \brief Run a single processing step. + * + * The processing step consists of receiving a new FRI monitoring message, + * calling the corresponding client callback and sending the resulting + * FRI command message back to the KUKA Sunrise controller. + * @return True if all of the substeps succeeded. + */ + bool step(); + + protected: + + IConnection& _connection; //!< connection interface + IClient* _robotClient; //!< robot client interface + TransformationClient* _trafoClient; //!< transformation client interface + ClientData* _data; //!< client data structure (for internal use) + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_APPLICATION_H + diff --git a/src/iiwa_controller/external/libFRI/include/friClientIf.h b/src/iiwa_controller/external/libFRI/include/friClientIf.h new file mode 100644 index 0000000..3c8ee24 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friClientIf.h @@ -0,0 +1,202 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_CLIENT_H +#define _KUKA_FRI_CLIENT_H + + + +/** Kuka namespace */ +namespace KUKA +{ +/** Fast Robot Interface (FRI) namespace */ +namespace FRI +{ + + // forward declarations + struct ClientData; + + + /** \brief Enumeration of the FRI session state. */ + enum ESessionState + { + IDLE = 0, //!< no session available + MONITORING_WAIT = 1, //!< monitoring mode with insufficient connection quality + MONITORING_READY = 2, //!< monitoring mode with connection quality sufficient for command mode + COMMANDING_WAIT = 3, //!< command mode about to start (overlay motion queued) + COMMANDING_ACTIVE = 4 //!< command mode active + }; + + /** \brief Enumeration of the FRI connection quality. */ + enum EConnectionQuality + { + POOR = 0, //!< poor connection quality + FAIR = 1, //!< connection quality insufficient for command mode + GOOD = 2, //!< connection quality sufficient for command mode + EXCELLENT = 3 //!< excellent connection quality + }; + + /** \brief Enumeration of the controller's safety state. */ + enum ESafetyState + { + NORMAL_OPERATION = 0, //!< No safety stop request present. + SAFETY_STOP_LEVEL_0 = 1,//!< Safety stop request STOP0 or STOP1 present. + SAFETY_STOP_LEVEL_1 = 2,//!< Safety stop request STOP1 (on-path) present. + SAFETY_STOP_LEVEL_2 = 3 //!< Safety stop request STOP2 present. + }; + + /** \brief Enumeration of the controller's current mode of operation. */ + enum EOperationMode + { + TEST_MODE_1 = 0, //!< test mode 1 with reduced speed (T1) + TEST_MODE_2 = 1, //!< test mode 2 (T2) + AUTOMATIC_MODE = 2 //!< automatic operation mode (AUT) + }; + + /** \brief Enumeration of a drive's state. */ + enum EDriveState + { + OFF = 0, //!< drive is not being used + TRANSITIONING = 1, //!< drive is in a transitioning state (before or after motion) + ACTIVE = 2 //!< drive is being actively commanded + }; + + /** \brief Enumeration of control mode. */ + enum EControlMode + { + POSITION_CONTROL_MODE = 0, //!< position control mode + CART_IMP_CONTROL_MODE = 1, //!< cartesian impedance control mode + JOINT_IMP_CONTROL_MODE = 2, //!< joint impedance control mode + NO_CONTROL = 3 //!< drives are not used + }; + + + /** \brief Enumeration of the client command mode. */ + enum EClientCommandMode + { + NO_COMMAND_MODE = 0, //!< no client command mode available + POSITION = 1, //!< commanding joint positions by the client + WRENCH = 2, //!< commanding wrenches and joint positions by the client + TORQUE = 3 //!< commanding joint torques and joint positions by the client + }; + + /** \brief Enumeration of the overlay type. */ + enum EOverlayType + { + NO_OVERLAY = 0, //!< no overlay type available + JOINT = 1, //!< joint overlay + CARTESIAN = 2 //!< cartesian overlay + }; + + + /** + * \brief FRI client interface. + * + * This is the callback interface that should be implemented by FRI clients. + * Callbacks are automatically called by the client application + * (ClientApplication) whenever new FRI messages arrive. + */ + class IClient + { + friend class ClientApplication; + + public: + + /** \brief Virtual destructor. */ + virtual ~IClient() {} + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState) = 0; + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand() = 0; + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command() = 0; + + protected: + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData() = 0; + + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_H diff --git a/src/iiwa_controller/external/libFRI/include/friConnectionIf.h b/src/iiwa_controller/external/libFRI/include/friConnectionIf.h new file mode 100644 index 0000000..c5502c2 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friConnectionIf.h @@ -0,0 +1,130 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_CONNECTION_H +#define _KUKA_FRI_CONNECTION_H + + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief FRI connection interface. + * + * Connections to the KUKA Sunrise controller have to be implemented using + * this interface. + */ + class IConnection + { + + public: + + /** \brief Virtual destructor. */ + virtual ~IConnection() {} + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID + * @param remoteHost The address of the remote host + * @return True if connection was established + */ + virtual bool open(int port, const char *remoteHost) = 0; + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close() = 0; + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const = 0; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, negative in case of errors) + */ + virtual int receive(char *buffer, int maxSize) = 0; + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size) = 0; + + }; + +} +} + + +#endif // _KUKA_FRI_CONNECTION_H diff --git a/src/iiwa_controller/external/libFRI/include/friException.h b/src/iiwa_controller/external/libFRI/include/friException.h new file mode 100644 index 0000000..719068d --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friException.h @@ -0,0 +1,153 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_EXCEPTION_H +#define _KUKA_FRI_EXCEPTION_H + +#include "stdio.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Standard exception for the FRI Client + * + * \note For realtime considerations the internal message buffer is static. + * So don't use this exception in more than one thread per process. + */ + class FRIException + { + + public: + + /** + * \brief FRIException Constructor + * + * @param message Error message + */ + FRIException(const char* message) + { + strncpy(_buffer, message, sizeof(_buffer) - 1); + _buffer[sizeof(_buffer) - 1] = 0; // ensure string termination + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain one "%s" parameter + * @param param1 First format parameter for parameter message. + */ + FRIException(const char* message, const char* param1) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief FRIException Constructor + * + * @param message Error message which may contain two "%s" parameter + * @param param1 First format parameter for parameter message. + * @param param2 Second format parameter for parameter message. + */ + FRIException(const char* message, const char* param1, const char* param2) + { +#ifdef _MSC_VER + _snprintf( // visual studio compilers (up to VS 2013) only know this method +#else + snprintf( +#endif + _buffer, sizeof(_buffer), message, param1, param2); + printf("FRIException: "); + printf(_buffer); + printf("\n"); + } + + /** + * \brief Get error string. + * @return Error message stored in the exception. + */ + const char* getErrorMessage() const { return _buffer; } + + /** \brief Virtual destructor. */ + virtual ~FRIException() {} + + protected: + static char _buffer[1024]; + + }; + +} +} + + +#endif // _KUKA_FRI_EXCEPTION_H diff --git a/src/iiwa_controller/external/libFRI/include/friLBRClient.h b/src/iiwa_controller/external/libFRI/include/friLBRClient.h new file mode 100644 index 0000000..5b461fa --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friLBRClient.h @@ -0,0 +1,145 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_LBR_CLIENT_H +#define _KUKA_FRI_LBR_CLIENT_H + +#include "friClientIf.h" +#include "friLBRState.h" +#include "friLBRCommand.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Implementation of the IClient interface for the KUKA LBR (lightweight) robots. + * + * Provides access to the current LBR state and the possibility to send new + * commands to the LBR. + */ + class LBRClient : public IClient + { + + public: + + /** \brief Constructor. */ + LBRClient(); + + /** \brief Destructor. */ + ~LBRClient(); + + /** + * \brief Callback that is called whenever the FRI session state changes. + * + * @param oldState previous FRI session state + * @param newState current FRI session state + */ + virtual void onStateChange(ESessionState oldState, ESessionState newState); + + /** + * \brief Callback for the FRI session states 'Monitoring Wait' and 'Monitoring Ready'. + */ + virtual void monitor(); + + /** + * \brief Callback for the FRI session state 'Commanding Wait'. + */ + virtual void waitForCommand(); + + /** + * \brief Callback for the FRI session state 'Commanding'. + */ + virtual void command(); + + /** + * \brief Provide read access to the current robot state. + * + * @return Reference to the LBRState instance + */ + const LBRState& robotState() const { return _robotState; } + + /** + * \brief Provide write access to the robot commands. + * + * @return Reference to the LBRCommand instance + */ + LBRCommand& robotCommand() { return _robotCommand; } + + private: + + LBRState _robotState; //!< wrapper class for the FRI monitoring message + LBRCommand _robotCommand; //!< wrapper class for the FRI command message + + /** + * \brief Method to create and initialize the client data structure (used internally). + * + * @return newly allocated client data structure + */ + virtual ClientData* createData(); + + }; + +} +} + + +#endif // _KUKA_FRI_LBR_CLIENT_H diff --git a/src/iiwa_controller/external/libFRI/include/friLBRCommand.h b/src/iiwa_controller/external/libFRI/include/friLBRCommand.h new file mode 100644 index 0000000..f6a1c27 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friLBRCommand.h @@ -0,0 +1,162 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_LBR_COMMAND_H +#define _KUKA_FRI_LBR_COMMAND_H + + +// forward declarations +typedef struct _FRICommandMessage FRICommandMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Wrapper class for the FRI command message for a KUKA LBR (leightweight) robot. + */ + class LBRCommand + { + friend class LBRClient; + + public: + + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param values Array with the new joint positions (in rad) + */ + void setJointPosition(const double* values); + + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param wrench Applied Cartesian wrench vector. + */ + void setWrench(const double* wrench); + + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param torques Array with the applied torque values (in Nm) + */ + void setTorque(const double* torques); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + + protected: + + static const int LBRCOMMANDMESSAGEID = 0x34001; //!< type identifier for the FRI command message corresponding to a KUKA LBR robot + FRICommandMessage* _cmdMessage; //!< FRI command message (protobuf struct) + FRIMonitoringMessage* _monMessage; //!< FRI monitoring message (protobuf struct) + + }; + +} +} + + +#endif // _KUKA_FRI_LBR_COMMAND_H diff --git a/src/iiwa_controller/external/libFRI/include/friLBRState.h b/src/iiwa_controller/external/libFRI/include/friLBRState.h new file mode 100644 index 0000000..76943c6 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friLBRState.h @@ -0,0 +1,278 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_LBR_STATE_H +#define _KUKA_FRI_LBR_STATE_H + +#include "friClientIf.h" + +// forward declarations +typedef struct _FRIMonitoringMessage FRIMonitoringMessage; + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief Wrapper class for the FRI monitoring message for a KUKA LBR (leightweight) robot. + */ + class LBRState + { + friend class LBRClient; + + public: + + enum + { + NUMBER_OF_JOINTS = 7 //!< number of axes of the KUKA LBR robot + }; + + LBRState(); + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI session state. + * + * @return current FRI session state + */ + ESessionState getSessionState() const; + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + * \brief Get the current safety state of the KUKA Sunrise controller. + * + * @return current safety state + */ + ESafetyState getSafetyState() const; + + /** + * \brief Get the current operation mode of the KUKA Sunrise controller. + * + * @return current operation mode + */ + EOperationMode getOperationMode() const; + + /** + * \brief Get the accumulated drive state over all drives of the KUKA LBR controller. + * + * If the drive states differ between drives, the following rule applies: + * 1) The drive state is OFF if all drives are OFF. + * 2) The drive state is ACTIVE if all drives are ACTIVE. + * 3) otherwise the drive state is TRANSITIONING. + * @return accumulated drive state + */ + EDriveState getDriveState() const; + + /** + * \brief Get the client command mode specified by the client. + * + * @return the client command mode specified by the client. + */ + EClientCommandMode getClientCommandMode() const; + + /** + * \brief Get the overlay type specified by the client. + * + * @return the overlay type specified by the client. + */ + EOverlayType getOverlayType() const; + + /** + * \brief Get the control mode of the KUKA LBR robot. + * + * @return current control mode of the KUKA LBR robot. + */ + EControlMode getControlMode() const; + + /** + * \brief Get the timestamp of the current robot state in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * @return timestamp encoded as Unix time (seconds) + */ + unsigned int getTimestampSec() const; + + /** + * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * @return timestamp encoded as Unix time (nanoseconds part) + */ + unsigned int getTimestampNanoSec() const; + + /** + * \brief Get the currently measured joint positions of the robot. + * + * @return array of the measured joint positions in radians + */ + const double* getMeasuredJointPosition() const; + + /** + * \brief Get the last commanded joint positions of the robot. + * + * @return array of the commanded joint positions in radians + */ + const double* getCommandedJointPosition() const; + + /** + * \brief Get the currently measured joint torques of the robot. + * + * @return array of the measured torques in Nm + */ + const double* getMeasuredTorque() const; + + /** + * \brief Get the last commanded joint torques of the robot. + * + * @return array of the commanded torques in Nm + */ + const double* getCommandedTorque() const; + + /** + * \brief Get the currently measured external joint torques of the robot. + * + * The external torques corresponds to the measured torques when removing + * the torques induced by the robot itself. + * @return array of the external torques in Nm + */ + const double* getExternalTorque() const; + + /** + * \brief Get the joint positions commanded by the interpolator. + * + * When commanding a motion overlay in your robot application, this method + * will give access to the joint positions currently commanded by the + * motion interpolator. + * @throw FRIException This method will throw an FRIException during monitoring mode. + * @return array of the ipo joint positions in radians + */ + const double* getIpoJointPosition() const; + + /** + * \brief Get an indicator for the current tracking performance of the commanded robot. + * + * The tracking performance is an indicator on how well the commanded robot + * is able to follow the commands of the FRI client. The best possible value + * 1.0 is reached when the robot executes the given commands instantaneously. + * The tracking performance drops towards 0 when latencies are induced, + * e.g. when the commanded velocity, acceleration or jerk exceeds the + * capabilities of the robot. + * The tracking performance will always be 0 when the session state does + * not equal COMMANDING_ACTIVE. + * @return current tracking performance + */ + double getTrackingPerformance() const; + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + protected: + + static const int LBRMONITORMESSAGEID = 0x245142; //!< type identifier for the FRI monitoring message corresponding to a KUKA LBR robot + FRIMonitoringMessage* _message; //!< FRI monitoring message (protobuf struct) + }; + +} +} + + +#endif // _KUKA_FRI_LBR_STATE_H diff --git a/src/iiwa_controller/external/libFRI/include/friTransformationClient.h b/src/iiwa_controller/external/libFRI/include/friTransformationClient.h new file mode 100644 index 0000000..6855c4f --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friTransformationClient.h @@ -0,0 +1,267 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_TRANSFORMATION_CLIENT_H +#define _KUKA_FRI_TRANSFORMATION_CLIENT_H + +#include +#include "friClientIf.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + // forward declaration + struct ClientData; + + /** + * \brief Abstract FRI transformation client. + * + * A transformation client enables the user to send transformation matrices cyclically to the + * KUKA Sunrise controller for manipulating the transformations of dynamic frames in the + * Sunrise scenegraph. + * Usually, these matrices will be provided by external sensors. + *
+ * Custom transformation clients have to be derived from this class and need to + * implement the provide() callback. This callback is called once by the + * client application whenever a new FRI message arrives. + * + * This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions. + */ + class TransformationClient + { + + friend class ClientApplication; + + public: + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Constructor. + **/ + TransformationClient(); + + /**
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Virtual destructor. + **/ + virtual ~TransformationClient(); + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Callback which is called whenever a new FRI message arrives. + * + * In this callback all requested transformations have to be set. + * + * \see getRequestedTransformationIDs(), setTransformation() + */ + virtual void provide() = 0; + + /** + * \brief Get the sample time in seconds. + * + * This is the period in which the KUKA Sunrise controller is sending + * FRI packets. + * @return sample time in seconds + */ + double getSampleTime() const; // sec + + /** + * \brief Get the current FRI connection quality. + * + * @return current FRI connection quality + */ + EConnectionQuality getConnectionQuality() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Returns a vector of identifiers of all requested transformation matrices. + * + * The custom TransformationClient has to provide data for transformation matrices with these + * identifiers. + * + * @return reference to vector of IDs of requested transformations + */ + const std::vector& getRequestedTransformationIDs() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * + * \brief Get the timestamp of the current received FRI monitor message in Unix time. + * + * This method returns the seconds since 0:00, January 1st, 1970 (UTC). + * Use getTimestampNanoSec() to increase your timestamp resolution when + * seconds are insufficient. + * + * @return timestamp encoded as Unix time (seconds) + */ + const unsigned int getTimestampSec() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Get the nanoseconds elapsed since the last second (in Unix time). + * + * This method complements getTimestampSec() to get a high resolution + * timestamp. + * + * @return timestamp encoded as Unix time (nanoseconds part) + */ + const unsigned int getTimestampNanoSec() const; + + /** + *
This element is an undocumented internal feature. It is not intended to be used by applications as it might change or be removed in future versions.
+ * \brief Provides a requested transformation matrix. + * + * A transformation matrix has 3x4 elements. It consists of a rotational matrix (3x3 elements) + * and a translational vector (3x1 elements). The complete transformation matrix has the + * following structure:
+ * [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ] + *

+ * All provided transformation matrices need a timestamp that corresponds to their + * time of acquisiton. This timestamp must be synchronized to the timestamp + * provided by the KUKA Sunrise controller (see getTimestampSec(), getTimestampNanoSec()). + *

+ * If an update to the last transformation is not yet available when the provide() + * callback is executed, the last transformation (including its timestamp) should be + * repeated until a new transformation is available. + * + * @throw FRIException Throws a FRIException if the maximum number of transformations is exceeded. + * @param transformationID Identifier string of the transformation matrix + * @param transformationMatrix Provided transformation matrix + * @param timeSec Timestamp encoded as Unix time (seconds) + * @param timeNanoSec Timestamp encoded as Unix time (nanoseconds part) + */ + void setTransformation(const char* transformationID, const double transformationMatrix[3][4], + unsigned int timeSec, unsigned int timeNanoSec); + + /** + * \brief Set boolean output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Boolean value to set. + */ + void setBooleanIOValue(const char* name, const bool value); + + /** + * \brief Set digital output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Digital value to set. + */ + void setDigitalIOValue(const char* name, const unsigned long long value); + + /** + * \brief Set analog output value. + * + * @throw FRIException Throws a FRIException if more outputs are set than can be registered. + * @throw FRIException May throw an FRIException if the IO is of wrong type, unknown or not an output. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @param value Analog value to set. + */ + void setAnalogIOValue(const char* name, const double value); + + /** + * \brief Get boolean IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's boolean value. + */ + bool getBooleanIOValue(const char* name) const; + + /** + * \brief Get digital IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's digital value. + */ + unsigned long long getDigitalIOValue(const char* name) const; + + /** + * \brief Get analog IO value. + * + * @throw FRIException May throw an FRIException if the IO is of wrong type or unknown. + * @param name Full name of the IO (Syntax "IOGroupName.IOName"). + * @return Returns IO's analog value. + */ + double getAnalogIOValue(const char* name) const; + + private: + + ClientData* _data; //!< the client data structure + + /** + * \brief Method to link the client data structure (used internally). + * + * @param clientData the client data structure + */ + void linkData(ClientData* clientData); + + }; + +} +} + +#endif // _KUKA_FRI_TRANSFORMATION_CLIENT_H diff --git a/src/iiwa_controller/external/libFRI/include/friUdpConnection.h b/src/iiwa_controller/external/libFRI/include/friUdpConnection.h new file mode 100644 index 0000000..23fe2ed --- /dev/null +++ b/src/iiwa_controller/external/libFRI/include/friUdpConnection.h @@ -0,0 +1,163 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_UDP_CONNECTION_H +#define _KUKA_FRI_UDP_CONNECTION_H + +#include + +#ifdef _WIN32 + #include +#else + // if linux or a other unix system is used, select uses the following include + #ifdef __unix__ + #include + #endif + // for VxWorks + #ifdef VXWORKS + #include + #include + #endif + #include + #include +#endif + +#include "friConnectionIf.h" + +/** Kuka namespace */ +namespace KUKA +{ +namespace FRI +{ + + /** + * \brief This class implements the IConnection interface using UDP sockets. + */ + class UdpConnection : public IConnection + { + + public: + + /** + * \brief Constructor with an optional parameter for setting a receive timeout. + * + * @param receiveTimeout Timeout (in ms) for receiving a UDP message (0 = wait forever) + * */ + UdpConnection(unsigned int receiveTimeout = 0); + + /** \brief Destructor. */ + ~UdpConnection(); + + /** + * \brief Open a connection to the KUKA Sunrise controller. + * + * @param port The port ID for the connection + * @param controllerAddress The IPv4 address of the KUKA Sunrise controller. + * If NULL, the FRI Client accepts connections from any + * address. + * @return True if connection was established, false otherwise + */ + virtual bool open(int port, const char *controllerAddress = NULL); + + /** + * \brief Close a connection to the KUKA Sunrise controller. + */ + virtual void close(); + + /** + * \brief Checks whether a connection to the KUKA Sunrise controller is established. + * + * @return True if connection is established + */ + virtual bool isOpen() const; + + /** + * \brief Receive a new FRI monitoring message from the KUKA Sunrise controller. + * + * This method blocks until a new message arrives. + * @param buffer Pointer to the allocated buffer that will hold the FRI message + * @param maxSize Size in bytes of the allocated buffer + * @return Number of bytes received (0 when connection was terminated, + * negative in case of errors or receive timeout) + */ + virtual int receive(char *buffer, int maxSize); + + /** + * \brief Send a new FRI command message to the KUKA Sunrise controller. + * + * @param buffer Pointer to the buffer holding the FRI message + * @param size Size in bytes of the message to be send + * @return True if successful + */ + virtual bool send(const char* buffer, int size); + + private: + + int _udpSock; //!< UDP socket handle + struct sockaddr_in _controllerAddr; //!< the controller's socket address + unsigned int _receiveTimeout; + fd_set _filedescriptor; + + }; + +} +} + + +#endif // _KUKA_FRI_UDP_CONNECTION_H diff --git a/src/iiwa_controller/external/libFRI/src/base/Makefile b/src/iiwa_controller/external/libFRI/src/base/Makefile new file mode 100644 index 0000000..291d151 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/base/Makefile @@ -0,0 +1,11 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CXX_SRC = friClientApplication.cpp + +INC_DIR += $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR) +CXXFLAGS += +LDFLAGS += + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/base/friClientApplication.cpp b/src/iiwa_controller/external/libFRI/src/base/friClientApplication.cpp new file mode 100644 index 0000000..ffc5166 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/base/friClientApplication.cpp @@ -0,0 +1,211 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include +#include "friClientApplication.h" +#include "friClientIf.h" +#include "friConnectionIf.h" +#include "friClientData.h" +#include "FRIMessages.pb.h" +#include "friTransformationClient.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client) + : _connection(connection), _robotClient(&client),_trafoClient(NULL), _data(NULL) +{ + _data = _robotClient->createData(); +} + +//****************************************************************************** +ClientApplication::ClientApplication(IConnection& connection, IClient& client, TransformationClient& trafoClient) + : _connection(connection), _robotClient(&client),_trafoClient(&trafoClient), _data(NULL) +{ + _data = _robotClient->createData(); + _trafoClient->linkData(_data); +} + +//****************************************************************************** +ClientApplication::~ClientApplication() +{ + disconnect(); + delete _data; +} + +//****************************************************************************** +bool ClientApplication::connect(int port, const char *remoteHost) +{ + if (_connection.isOpen()) + { + printf("Warning: client application already connected!\n"); + return true; + } + + return _connection.open(port, remoteHost); +} + +//****************************************************************************** +void ClientApplication::disconnect() +{ + if (_connection.isOpen()) _connection.close(); +} + +//****************************************************************************** +bool ClientApplication::step() +{ + if (!_connection.isOpen()) + { + printf("Error: client application is not connected!\n"); + return false; + } + + // ************************************************************************** + // Receive and decode new monitoring message + // ************************************************************************** + int size = _connection.receive(_data->receiveBuffer, FRI_MONITOR_MSG_MAX_SIZE); + + if (size <= 0) + { // TODO: size == 0 -> connection closed (maybe go to IDLE instead of stopping?) + printf("Error: failed while trying to receive monitoring message!\n"); + return false; + } + + if (!_data->decoder.decode(_data->receiveBuffer, size)) + { + return false; + } + + // check message type (so that our wrappers match) + if (_data->expectedMonitorMsgID != _data->monitoringMsg.header.messageIdentifier) + { + printf("Error: incompatible IDs for received message (got: %d expected %d)!\n", + (int)_data->monitoringMsg.header.messageIdentifier, + (int)_data->expectedMonitorMsgID); + return false; + } + + // ************************************************************************** + // callbacks + // ************************************************************************** + // reset commmand message before callbacks + _data->resetCommandMessage(); + + // callbacks for robot client + ESessionState currentState = (ESessionState)_data->monitoringMsg.connectionInfo.sessionState; + + if (_data->lastState != currentState) + { + _robotClient->onStateChange(_data->lastState, currentState); + _data->lastState = currentState; + } + + switch (currentState) + { + case MONITORING_WAIT: + case MONITORING_READY: + _robotClient->monitor(); + break; + case COMMANDING_WAIT: + _robotClient->waitForCommand(); + break; + case COMMANDING_ACTIVE: + _robotClient->command(); + break; + case IDLE: + default: + return true; // nothing to send back + } + + // callback for transformation client + if(_trafoClient != NULL) + { + _trafoClient->provide(); + } + + // ************************************************************************** + // Encode and send command message + // ************************************************************************** + + _data->lastSendCounter++; + // check if its time to send an answer + if (_data->lastSendCounter >= _data->monitoringMsg.connectionInfo.receiveMultiplier) + { + _data->lastSendCounter = 0; + + // set sequence counters + _data->commandMsg.header.sequenceCounter = _data->sequenceCounter++; + _data->commandMsg.header.reflectedSequenceCounter = + _data->monitoringMsg.header.sequenceCounter; + + if (!_data->encoder.encode(_data->sendBuffer, size)) + { + return false; + } + + if (!_connection.send(_data->sendBuffer, size)) + { + printf("Error: failed while trying to send command message!\n"); + return false; + } + } + + return true; +} + diff --git a/src/iiwa_controller/external/libFRI/src/base/friClientData.h b/src/iiwa_controller/external/libFRI/src/base/friClientData.h new file mode 100644 index 0000000..03e456c --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/base/friClientData.h @@ -0,0 +1,240 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_CLIENT_DATA_H +#define _KUKA_FRI_CLIENT_DATA_H + +#include + +#include "FRIMessages.pb.h" +#include "friMonitoringMessageDecoder.h" +#include "friCommandMessageEncoder.h" +#include "friClientIf.h" +#include "friException.h" + +namespace KUKA +{ +namespace FRI +{ + + struct ClientData + { + char receiveBuffer[FRI_MONITOR_MSG_MAX_SIZE];//!< monitoring message receive buffer + char sendBuffer[FRI_COMMAND_MSG_MAX_SIZE]; //!< command message send buffer + + FRIMonitoringMessage monitoringMsg; //!< monitoring message struct + FRICommandMessage commandMsg; //!< command message struct + + MonitoringMessageDecoder decoder; //!< monitoring message decoder + CommandMessageEncoder encoder; //!< command message encoder + + ESessionState lastState; //!< last FRI state + uint32_t sequenceCounter; //!< sequence counter for command messages + uint32_t lastSendCounter; //!< steps since last send command + uint32_t expectedMonitorMsgID; //!< expected ID for received monitoring messages + + const size_t MAX_REQUESTED_TRANSFORMATIONS; //!< maximum count of requested transformations + const size_t MAX_SIZE_TRANSFORMATION_ID; //!< maximum size in bytes of a transformation ID + std::vector requestedTrafoIDs; //!< list of requested transformation ids + + ClientData(int numDofs) + : decoder(&monitoringMsg, numDofs), + encoder(&commandMsg, numDofs), + lastState(IDLE), + sequenceCounter(0), + lastSendCounter(0), + expectedMonitorMsgID(0), + MAX_REQUESTED_TRANSFORMATIONS(sizeof(monitoringMsg.requestedTransformations) / + sizeof(monitoringMsg.requestedTransformations[0])), + MAX_SIZE_TRANSFORMATION_ID(sizeof(monitoringMsg.requestedTransformations[0].name)) + { + requestedTrafoIDs.reserve(MAX_REQUESTED_TRANSFORMATIONS); + } + + void resetCommandMessage() + { + commandMsg.commandData.has_jointPosition = false; + commandMsg.commandData.has_cartesianWrenchFeedForward = false; + commandMsg.commandData.has_jointTorque = false; + commandMsg.commandData.commandedTransformations_count = 0; + commandMsg.has_commandData = false; + commandMsg.commandData.writeIORequest_count = 0; + } + + //****************************************************************************** + static const FriIOValue& getBooleanIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_BOOLEAN); + } + + //****************************************************************************** + static const FriIOValue& getDigitalIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_DIGITAL); + } + + //****************************************************************************** + static const FriIOValue& getAnalogIOValue(const FRIMonitoringMessage* message, const char* name) + { + return getIOValue(message, name, FriIOType_ANALOG); + } + + //****************************************************************************** + static void setBooleanIOValue(FRICommandMessage* message, const char* name, const bool value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_BOOLEAN).digitalValue = value; + } + + //****************************************************************************** + static void setDigitalIOValue(FRICommandMessage* message, const char* name, const unsigned long long value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_DIGITAL).digitalValue = value; + } + + //****************************************************************************** + static void setAnalogIOValue(FRICommandMessage* message, const char* name, const double value, + const FRIMonitoringMessage* monMessage) + { + setIOValue(message, name, monMessage, FriIOType_ANALOG).analogValue = value; + } + + protected: + + //****************************************************************************** + static const FriIOValue& getIOValue(const FRIMonitoringMessage* message, const char* name, + const FriIOType ioType) + { + if(message != NULL && message->has_monitorData == true) + { + const MessageMonitorData& monData = message->monitorData; + const bool analogValue = (ioType == FriIOType_ANALOG); + const bool digitalValue = (ioType == FriIOType_DIGITAL | ioType == FriIOType_BOOLEAN); + for(size_t i = 0; i < monData.readIORequest_count; i++) + { + const FriIOValue& ioValue = monData.readIORequest[i]; + if(strcmp(name, ioValue.name) == 0) + { + if(ioValue.type == ioType && + ioValue.has_digitalValue == digitalValue && + ioValue.has_analogValue == analogValue) + { + return ioValue; + } + + const char* ioTypeName; + switch(ioType) + { + case FriIOType_ANALOG: ioTypeName = "analog value"; break; + case FriIOType_DIGITAL: ioTypeName = "digital value"; break; + case FriIOType_BOOLEAN: ioTypeName = "boolean"; break; + default: ioTypeName = "?"; break; + } + + throw FRIException("IO %s is not of type %s.", name, ioTypeName); + } + } + } + + throw FRIException("Could not locate IO %s in monitor message.", name); + } + + //****************************************************************************** + static FriIOValue& setIOValue(FRICommandMessage* message, const char* name, + const FRIMonitoringMessage* monMessage, const FriIOType ioType) + { + MessageCommandData& cmdData = message->commandData; + const size_t maxIOs = sizeof(cmdData.writeIORequest) / sizeof(cmdData.writeIORequest[0]); + if(cmdData.writeIORequest_count < maxIOs) + { + // call getter which will raise an exception if the output doesn't exist + // or is of wrong type. + if(getIOValue(monMessage, name, ioType).direction != FriIODirection_OUTPUT) + { + throw FRIException("IO %s is not an output value.", name); + } + + // add IO value to command message + FriIOValue& ioValue = cmdData.writeIORequest[cmdData.writeIORequest_count]; + + strncpy(ioValue.name, name, sizeof(ioValue.name) - 1); + ioValue.name[sizeof(ioValue.name) - 1] = 0; // ensure termination + ioValue.type = ioType; + ioValue.has_digitalValue = (ioType == FriIOType_DIGITAL | ioType == FriIOType_BOOLEAN); + ioValue.has_analogValue = (ioType == FriIOType_ANALOG); + ioValue.direction = FriIODirection_OUTPUT; + + cmdData.writeIORequest_count ++; + message->has_commandData = true; + + return ioValue; + } + else + { + throw FRIException("Exceeded maximum number of IOs that can be set."); + } + } + }; + +} +} + + +#endif // _KUKA_FRI_CLIENT_DATA_H diff --git a/src/iiwa_controller/external/libFRI/src/client_lbr/Makefile b/src/iiwa_controller/external/libFRI/src/client_lbr/Makefile new file mode 100644 index 0000000..214caaa --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_lbr/Makefile @@ -0,0 +1,13 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CXX_SRC = friLBRClient.cpp \ + friLBRCommand.cpp \ + friLBRState.cpp + +INC_DIR += $(CLIENTBASE_DIR) $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR) +CXXFLAGS += +LDFLAGS += + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRClient.cpp b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRClient.cpp new file mode 100644 index 0000000..67bac9e --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRClient.cpp @@ -0,0 +1,120 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include +#include "friLBRClient.h" +#include "friClientData.h" + +using namespace KUKA::FRI; +char FRIException::_buffer[1024] = { 0 }; + +//****************************************************************************** +LBRClient::LBRClient() +{ + +} + +//****************************************************************************** +LBRClient::~LBRClient() +{ + +} + +//****************************************************************************** +void LBRClient::onStateChange(ESessionState oldState, ESessionState newState) +{ + // TODO: String converter function for states + printf("LBRiiwaClient state changed from %d to %d\n", oldState, newState); +} + +//****************************************************************************** +void LBRClient::monitor() +{ + robotCommand().setJointPosition(robotState().getCommandedJointPosition()); +} + +//****************************************************************************** +void LBRClient::waitForCommand() +{ + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +void LBRClient::command() +{ + robotCommand().setJointPosition(robotState().getIpoJointPosition()); +} + +//****************************************************************************** +ClientData* LBRClient::createData() +{ + ClientData* data = new ClientData(_robotState.NUMBER_OF_JOINTS); + + // link monitoring and command message to wrappers + _robotState._message = &data->monitoringMsg; + _robotCommand._cmdMessage = &data->commandMsg; + _robotCommand._monMessage = &data->monitoringMsg; + + // set specific message IDs + data->expectedMonitorMsgID = _robotState.LBRMONITORMESSAGEID; + data->commandMsg.header.messageIdentifier = _robotCommand.LBRCOMMANDMESSAGEID; + + return data; +} + diff --git a/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRCommand.cpp b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRCommand.cpp new file mode 100644 index 0000000..61f0ddf --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRCommand.cpp @@ -0,0 +1,113 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include "friLBRState.h" +#include "friLBRCommand.h" +#include "friClientData.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +void LBRCommand::setJointPosition(const double* values) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointPosition = true; + tRepeatedDoubleArguments *dest = + (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointPosition.value.arg; + memcpy(dest->value, values, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setWrench(const double* wrench) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_cartesianWrenchFeedForward = true; + + double *dest = _cmdMessage->commandData.cartesianWrenchFeedForward.element; + memcpy(dest, wrench, 6 * sizeof(double)); +} +//****************************************************************************** +void LBRCommand::setTorque(const double* torques) +{ + _cmdMessage->has_commandData = true; + _cmdMessage->commandData.has_jointTorque= true; + + tRepeatedDoubleArguments *dest = + (tRepeatedDoubleArguments*)_cmdMessage->commandData.jointTorque.value.arg; + memcpy(dest->value, torques, LBRState::NUMBER_OF_JOINTS * sizeof(double)); +} + +//****************************************************************************** +void LBRCommand::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(_cmdMessage, name, value, _monMessage); +} + +//****************************************************************************** +void LBRCommand::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(_cmdMessage, name, value, _monMessage); +} diff --git a/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRState.cpp b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRState.cpp new file mode 100644 index 0000000..2b25fe8 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_lbr/friLBRState.cpp @@ -0,0 +1,234 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include "friLBRState.h" +#include "friClientData.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + + +LBRState::LBRState():_message(0) +{ + +} +//****************************************************************************** +double LBRState::getSampleTime() const +{ + return _message->connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +ESessionState LBRState::getSessionState() const +{ + return (ESessionState)_message->connectionInfo.sessionState; +} + +//****************************************************************************** +EConnectionQuality LBRState::getConnectionQuality() const +{ + return (EConnectionQuality)_message->connectionInfo.quality; +} + +//****************************************************************************** +ESafetyState LBRState::getSafetyState() const +{ + return (ESafetyState)_message->robotInfo.safetyState; +} + +//****************************************************************************** +EOperationMode LBRState::getOperationMode() const +{ + return (EOperationMode)_message->robotInfo.operationMode; +} + +//****************************************************************************** +EDriveState LBRState::getDriveState() const +{ + tRepeatedIntArguments *values = + (tRepeatedIntArguments *)_message->robotInfo.driveState.arg; + int firstState = (int)values->value[0]; + for (int i=1; ivalue[i]; + if (state != firstState) + { + return TRANSITIONING; + } + } + return (EDriveState)firstState; +} + + +//******************************************************************************** +EOverlayType LBRState::getOverlayType() const +{ + return (EOverlayType)_message->ipoData.overlayType; +} + +//******************************************************************************** +EClientCommandMode LBRState::getClientCommandMode() const +{ + return (EClientCommandMode)_message->ipoData.clientCommandMode; +} + + +//****************************************************************************** +EControlMode LBRState::getControlMode() const +{ + return (EControlMode)_message->robotInfo.controlMode; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampSec() const +{ + return _message->monitorData.timestamp.sec; +} + +//****************************************************************************** +unsigned int LBRState::getTimestampNanoSec() const +{ + return _message->monitorData.timestamp.nanosec; +} + +//****************************************************************************** +const double* LBRState::getMeasuredJointPosition() const +{ + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->monitorData.measuredJointPosition.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +const double* LBRState::getCommandedJointPosition() const +{ + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->monitorData.commandedJointPosition.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +const double* LBRState::getMeasuredTorque() const +{ + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->monitorData.measuredTorque.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +const double* LBRState::getCommandedTorque() const +{ + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->monitorData.commandedTorque.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +const double* LBRState::getExternalTorque() const +{ + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->monitorData.externalTorque.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +const double* LBRState::getIpoJointPosition() const +{ + if (!_message->ipoData.has_jointPosition) + { + throw FRIException("IPO joint position not available in monitoring mode."); + return NULL; + } + + tRepeatedDoubleArguments *values = + (tRepeatedDoubleArguments*)_message->ipoData.jointPosition.value.arg; + return (double*)values->value; +} + +//****************************************************************************** +double LBRState::getTrackingPerformance() const +{ + if (!_message->ipoData.has_trackingPerformance) return 0.0; + + return _message->ipoData.trackingPerformance; +} + +//****************************************************************************** +bool LBRState::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(_message, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long LBRState::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(_message, name).digitalValue; +} + +//****************************************************************************** +double LBRState::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(_message, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& LBRState::getRequestedIO_IDs() const +{ + return _clientData->getRequestedIO_IDs(); +}*/ diff --git a/src/iiwa_controller/external/libFRI/src/client_trafo/Makefile b/src/iiwa_controller/external/libFRI/src/client_trafo/Makefile new file mode 100644 index 0000000..a5b119e --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_trafo/Makefile @@ -0,0 +1,11 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CXX_SRC = friTransformationClient.cpp + +INC_DIR += $(CLIENTBASE_DIR) $(NANOPB_DIR) $(PROTOBUF_DIR) $(PROTOBUF_GEN_DIR) +CXXFLAGS += +LDFLAGS += + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/client_trafo/friTransformationClient.cpp b/src/iiwa_controller/external/libFRI/src/client_trafo/friTransformationClient.cpp new file mode 100644 index 0000000..38c80fe --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/client_trafo/friTransformationClient.cpp @@ -0,0 +1,191 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ + +#include +#include + +#include "friTransformationClient.h" +#include "friClientData.h" + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +TransformationClient::TransformationClient() +{ +} + +//****************************************************************************** +TransformationClient::~TransformationClient() +{ +} + +//****************************************************************************** +const std::vector& TransformationClient::getRequestedTransformationIDs() const +{ + unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count; + _data->requestedTrafoIDs.resize(trafoCount); + for (unsigned int i=0; irequestedTrafoIDs[i] = _data->monitoringMsg.requestedTransformations[i].name; + } + return _data->requestedTrafoIDs; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.sec; +} + +//****************************************************************************** +const unsigned int TransformationClient::getTimestampNanoSec() const +{ + return _data->monitoringMsg.monitorData.timestamp.nanosec; +} + +//****************************************************************************** +void TransformationClient::setTransformation(const char* transformationID, + const double transformationMatrix[3][4], unsigned int timeSec, unsigned int timeNanoSec) +{ + _data->commandMsg.has_commandData = true; + + unsigned int currentSize = _data->commandMsg.commandData.commandedTransformations_count; + + if (currentSize < _data->MAX_REQUESTED_TRANSFORMATIONS) + { + _data->commandMsg.commandData.commandedTransformations_count++; + Transformation& dest = _data->commandMsg.commandData.commandedTransformations[currentSize]; + strncpy(dest.name, transformationID, _data->MAX_SIZE_TRANSFORMATION_ID); + dest.name[_data->MAX_SIZE_TRANSFORMATION_ID - 1] = '\0'; + dest.matrix_count = 12; + memcpy(dest.matrix, transformationMatrix, 12 * sizeof(double)); + dest.has_timestamp = true; + dest.timestamp.sec = timeSec; + dest.timestamp.nanosec = timeNanoSec; + } + else + { + throw FRIException("Exceeded maximum number of transformations."); + } +} + +//****************************************************************************** +void TransformationClient::linkData(ClientData* clientData) +{ + _data = clientData; +} + +//****************************************************************************** +double TransformationClient::getSampleTime() const +{ + return _data->monitoringMsg.connectionInfo.sendPeriod * 0.001; +} + +//****************************************************************************** +EConnectionQuality TransformationClient::getConnectionQuality() const +{ + return (EConnectionQuality)_data->monitoringMsg.connectionInfo.quality; +} + + +//****************************************************************************** +void TransformationClient::setBooleanIOValue(const char* name, const bool value) +{ + ClientData::setBooleanIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setAnalogIOValue(const char* name, const double value) +{ + ClientData::setAnalogIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +void TransformationClient::setDigitalIOValue(const char* name, const unsigned long long value) +{ + ClientData::setDigitalIOValue(&_data->commandMsg, name, value, &_data->monitoringMsg); +} + +//****************************************************************************** +bool TransformationClient::getBooleanIOValue(const char* name) const +{ + return ClientData::getBooleanIOValue(&_data->monitoringMsg, name).digitalValue != 0; +} + +//****************************************************************************** +unsigned long long TransformationClient::getDigitalIOValue(const char* name) const +{ + return ClientData::getDigitalIOValue(&_data->monitoringMsg, name).digitalValue; +} + +//****************************************************************************** +double TransformationClient::getAnalogIOValue(const char* name) const +{ + return ClientData::getAnalogIOValue(&_data->monitoringMsg, name).analogValue; +} + +//****************************************************************************** +/*const std::vector& TransformationClient::getRequestedIO_IDs() const +{ + return _data->getRequestedIO_IDs(); +}*/ diff --git a/src/iiwa_controller/external/libFRI/src/connection/Makefile b/src/iiwa_controller/external/libFRI/src/connection/Makefile new file mode 100644 index 0000000..1995d06 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/connection/Makefile @@ -0,0 +1,14 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CXX_SRC = friUdpConnection.cpp + +CXXFLAGS += +LDFLAGS += + +################################################################################ +### Include general makefile (at the end) +################################################################################ + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/connection/friUdpConnection.cpp b/src/iiwa_controller/external/libFRI/src/connection/friUdpConnection.cpp new file mode 100644 index 0000000..4c79f06 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/connection/friUdpConnection.cpp @@ -0,0 +1,243 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software �KUKA Sunrise.Connectivity FRI Client SDK� is targeted to work in +conjunction with the �KUKA Sunrise.Connectivity FastRobotInterface� toolkit. +In the following, the term �software� refers to all material directly +belonging to the provided SDK �Software development kit�, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include +#include +#ifndef _MSC_VER +#include +#endif + +#include "friUdpConnection.h" + + +#ifdef WIN32 +#include +#include +#ifdef _MSC_VER +#pragma comment(lib, "ws2_32.lib") +#endif +#endif + +using namespace KUKA::FRI; + +//****************************************************************************** +UdpConnection::UdpConnection(unsigned int receiveTimeout) : + _udpSock(-1), + _receiveTimeout(receiveTimeout) +{ +#ifdef WIN32 + WSADATA WSAData; + WSAStartup(MAKEWORD(2,0), &WSAData); +#endif +} + +//****************************************************************************** +UdpConnection::~UdpConnection() +{ + close(); +#ifdef WIN32 + WSACleanup(); +#endif +} + +//****************************************************************************** +bool UdpConnection::open(int port, const char *controllerAddress) +{ + struct sockaddr_in servAddr; + memset(&servAddr, 0, sizeof(servAddr)); + memset(&_controllerAddr, 0, sizeof(_controllerAddr)); + + // socket creation + _udpSock = socket(PF_INET, SOCK_DGRAM, 0); + if (_udpSock < 0) + { + printf("opening socket failed!\n"); + return false; + } + + // use local server port + servAddr.sin_family = AF_INET; + servAddr.sin_port = htons(port); + servAddr.sin_addr.s_addr = htonl(INADDR_ANY); + + if (bind(_udpSock, (struct sockaddr *)&servAddr, sizeof(servAddr)) < 0) + { + printf("binding port number %d failed!\n", port); + close(); + return false; + } + // initialize the socket properly + _controllerAddr.sin_family = AF_INET; + _controllerAddr.sin_port = htons(port); + if (controllerAddress) + { +#ifndef __MINGW32__ + inet_pton(AF_INET, controllerAddress, &_controllerAddr.sin_addr); +#else + _controllerAddr.sin_addr.s_addr = inet_addr(controllerAddress); +#endif + if ( connect(_udpSock, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)) < 0) + { + printf("connecting to controller with address %s failed !\n", controllerAddress); + close(); + return false; + } + } + else + { + _controllerAddr.sin_addr.s_addr = htonl(INADDR_ANY); + } + return true; +} + +//****************************************************************************** +void UdpConnection::close() +{ + if (isOpen()) + { +#ifdef WIN32 + closesocket(_udpSock); +#else + ::close(_udpSock); +#endif + } + _udpSock = -1; +} + +//****************************************************************************** +bool UdpConnection::isOpen() const +{ + return (_udpSock >= 0); +} + +//****************************************************************************** +int UdpConnection::receive(char *buffer, int maxSize) +{ + if (isOpen()) + { + /** HAVE_SOCKLEN_T + Yes - unbelievable: There are differences in standard calling parameters (types) to recvfrom + Windows winsock, VxWorks and QNX use int + newer Posix (most Linuxes) use socklen_t + */ +#ifdef HAVE_SOCKLEN_T + socklen_t sockAddrSize; +#else + int sockAddrSize; +#endif + sockAddrSize = static_cast(sizeof(struct sockaddr_in)); + /** check for timeout + Because SO_RCVTIMEO is in Windows not correctly implemented, select is used for the receive time out. + If a timeout greater than 0 is given, wait until the timeout is reached or a message was received. + If t, abort the function with an error. + */ + if(_receiveTimeout > 0) + { + + // Set up struct timeval + struct timeval tv; + tv.tv_sec = _receiveTimeout / 1000; + tv.tv_usec = (_receiveTimeout % 1000) * 1000; + + // initialize file descriptor + /** + * Replace FD_ZERO with memset, because bzero is not available for VxWorks + * User Space Aplications(RTPs). Therefore the macro FD_ZERO does not compile. + */ +#ifndef VXWORKS + FD_ZERO(&_filedescriptor); +#else + memset((char *)(&_filedescriptor), 0, sizeof(*(&_filedescriptor))); +#endif + FD_SET(_udpSock, &_filedescriptor); + + // wait until something was received + int numberActiveFileDescr = select(_udpSock+1, &_filedescriptor,NULL,NULL,&tv); + // 0 indicates a timeout + if(numberActiveFileDescr == 0) + { + printf("The connection has timed out. Timeout is %d\n", _receiveTimeout); + return -1; + } + // a negative value indicates an error + else if(numberActiveFileDescr == -1) + { + printf("An error has occured \n"); + return -1; + } + } + + return recvfrom(_udpSock, buffer, maxSize, 0, (struct sockaddr *)&_controllerAddr, &sockAddrSize); + } + return -1; +} + +//****************************************************************************** +bool UdpConnection::send(const char* buffer, int size) +{ + if ((isOpen()) && (ntohs(_controllerAddr.sin_port) != 0)) + { + int sent = sendto(_udpSock, const_cast(buffer), size, 0, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr)); + if (sent == size) + { + return true; + } + } + return false; +} diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/CHANGELOG.txt b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/CHANGELOG.txt new file mode 100644 index 0000000..204a45d --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/CHANGELOG.txt @@ -0,0 +1,150 @@ +nanopb-0.2.8 (2014-05-20) + Fix security issue with PB_ENABLE_MALLOC. (issue 117) + Add option to not add timestamps to .pb.h and .pb.c preambles. (issue 115) + Documentation updates + Improved tests + +nanopb-0.2.7 (2014-04-07) + Fix bug with default values for extension fields (issue 111) + Fix some MISRA-C warnings (issue 91) + Implemented optional malloc() support (issue 80) + Changed pointer-type bytes field datatype + Add a "found" field to pb_extension_t (issue 112) + Add convenience function pb_get_encoded_size() (issue 16) + +nanopb-0.2.6 (2014-02-15) + Fix generator error with bytes callback fields (issue 99) + Fix warnings about large integer constants (issue 102) + Add comments to where STATIC_ASSERT is used (issue 96) + Add warning about unknown field names on .options (issue 105) + Move descriptor.proto to google/protobuf subdirectory (issue 104) + Improved tests + +nanopb-0.2.5 (2014-01-01) + Fix a bug with encoding negative values in int32 fields (issue 97) + Create binary packages of the generator + dependencies (issue 47) + Add support for pointer-type fields to the encoder (part of issue 80) + Fixed path in FindNanopb.cmake (issue 94) + Improved tests + +nanopb-0.2.4 (2013-11-07) + Remove the deprecated NANOPB_INTERNALS functions from public API. + Document the security model. + Check array and bytes max sizes when encoding (issue 90) + Add #defines for maximum encoded message size (issue 89) + Add #define tags for extension fields (issue 93) + Fix MISRA C violations (issue 91) + Clean up pb_field_t definition with typedefs. + +nanopb-0.2.3 (2013-09-18) + Improve compatibility by removing ternary operator from initializations (issue 88) + Fix build error on Visual C++ (issue 84, patch by Markus Schwarzenberg) + Don't stop on unsupported extension fields (issue 83) + Add an example pb_syshdr.h file for non-C99 compilers + Reorganize tests and examples into subfolders (issue 63) + Switch from Makefiles to scons for building the tests + Make the tests buildable on Windows + +nanopb-0.2.2 (2013-08-18) + Add support for extension fields (issue 17) + Fix unknown fields in empty message (issue 78) + Include the field tags in the generated .pb.h file. + Add pb_decode_delimited and pb_encode_delimited wrapper functions (issue 74) + Add a section in top of pb.h for changing compilation settings (issue 76) + Documentation improvements (issues 12, 77 and others) + Improved tests + +nanopb-0.2.1 (2013-04-14) + NOTE: The default callback function signature has changed. + If you don't want to update your code, define PB_OLD_CALLBACK_STYLE. + + Change the callback function to use void** (issue 69) + Add support for defining the nanopb options in a separate file (issue 12) + Add support for packed structs in IAR and MSVC (in addition to GCC) (issue 66) + Implement error message support for the encoder side (issue 7) + Handle unterminated strings when encoding (issue 68) + Fix bug with empty strings in repeated string callbacks (issue 73) + Fix regression in 0.2.0 with optional callback fields (issue 70) + Fix bugs with empty message types (issues 64, 65) + Fix some compiler warnings on clang (issue 67) + Some portability improvements (issues 60, 62) + Various new generator options + Improved tests + +nanopb-0.2.0 (2013-03-02) + NOTE: This release requires you to regenerate all .pb.c + files. Files generated by older versions will not + compile anymore. + + Reformat generated .pb.c files using macros (issue 58) + Rename PB_HTYPE_ARRAY -> PB_HTYPE_REPEATED + Separate PB_HTYPE to PB_ATYPE and PB_HTYPE + Move STATIC_ASSERTs to .pb.c file + Added CMake file (by Pavel Ilin) + Add option to give file extension to generator (by Michael Haberler) + Documentation updates + +nanopb-0.1.9 (2013-02-13) + Fixed error message bugs (issues 52, 56) + Sanitize #ifndef filename (issue 50) + Performance improvements + Add compile-time option PB_BUFFER_ONLY + Add Java package name to nanopb.proto + Check for sizeof(double) == 8 (issue 54) + Added generator option to ignore some fields. (issue 51) + Added generator option to make message structs packed. (issue 49) + Add more test cases. + +nanopb-0.1.8 (2012-12-13) + Fix bugs in the enum short names introduced in 0.1.7 (issues 42, 43) + Fix STATIC_ASSERT macro when using multiple .proto files. (issue 41) + Fix missing initialization of istream.errmsg + Make tests/Makefile work for non-gcc compilers (issue 40) + +nanopb-0.1.7 (2012-11-11) + Remove "skip" mode from pb_istream_t callbacks. Example implementation had a bug. (issue 37) + Add option to use shorter names for enum values (issue 38) + Improve options support in generator (issues 12, 30) + Add nanopb version number to generated files (issue 36) + Add extern "C" to generated headers (issue 35) + Add names for structs to allow forward declaration (issue 39) + Add buffer size check in example (issue 34) + Fix build warnings on MS compilers (issue 33) + +nanopb-0.1.6 (2012-09-02) + Reorganize the field decoder interface (issue 2) + Improve performance in submessage decoding (issue 28) + Implement error messages in the decoder side (issue 7) + Extended testcases (alltypes test is now complete). + Fix some compiler warnings (issues 25, 26, 27, 32). + +nanopb-0.1.5 (2012-08-04) + Fix bug in decoder with packed arrays (issue 23). + Extended testcases. + Fix some compiler warnings. + +nanopb-0.1.4 (2012-07-05) + Add compile-time options for easy-to-use >255 field support. + Improve the detection of missing required fields. + Added example on how to handle union messages. + Fix generator error with .proto without messages. + Fix problems that stopped the code from compiling with some compilers. + Fix some compiler warnings. + +nanopb-0.1.3 (2012-06-12) + Refactor the field encoder interface. + Improve generator error messages (issue 5) + Add descriptor.proto into the #include exclusion list + Fix some compiler warnings. + +nanopb-0.1.2 (2012-02-15) + Make the generator to generate include for other .proto files (issue 4). + Fixed generator not working on Windows (issue 3) + +nanopb-0.1.1 (2012-01-14) + Fixed bug in encoder with 'bytes' fields (issue 1). + Fixed a bug in the generator that caused a compiler error on sfixed32 and sfixed64 fields. + Extended testcases. + +nanopb-0.1.0 (2012-01-06) + First stable release. diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/LICENSE.txt b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/LICENSE.txt new file mode 100644 index 0000000..53e0e3a --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/LICENSE.txt @@ -0,0 +1,20 @@ +Copyright (c) 2011 Petteri Aimonen + +This software is provided 'as-is', without any express or +implied warranty. In no event will the authors be held liable +for any damages arising from the use of this software. + +Permission is granted to anyone to use this software for any +purpose, including commercial applications, and to alter it and +redistribute it freely, subject to the following restrictions: + +1. The origin of this software must not be misrepresented; you + must not claim that you wrote the original software. If you use + this software in a product, an acknowledgment in the product + documentation would be appreciated but is not required. + +2. Altered source versions must be plainly marked as such, and + must not be misrepresented as being the original software. + +3. This notice may not be removed or altered from any source + distribution. diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/Makefile b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/Makefile new file mode 100644 index 0000000..136521a --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/Makefile @@ -0,0 +1,15 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CC_SRC = pb_encode.c \ + pb_decode.c + +CFLAGS += +LDFLAGS += + +################################################################################ +### Include general makefile (at the end) +################################################################################ + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/README.txt b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/README.txt new file mode 100644 index 0000000..770ab1a --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/README.txt @@ -0,0 +1,61 @@ +Nanopb is a small code-size Protocol Buffers implementation in ansi C. It is +especially suitable for use in microcontrollers, but fits any memory +restricted system. + +Homepage: http://kapsi.fi/~jpa/nanopb/ + + + + +Using the nanopb library +======================== +To use the nanopb library, you need to do two things: + +1) Compile your .proto files for nanopb, using protoc. +2) Include pb_encode.c and pb_decode.c in your project. + +The easiest way to get started is to study the project in "examples/simple". +It contains a Makefile, which should work directly under most Linux systems. +However, for any other kind of build system, see the manual steps in +README.txt in that folder. + + + +Using the Protocol Buffers compiler (protoc) +============================================ +The nanopb generator is implemented as a plugin for the Google's own protoc +compiler. This has the advantage that there is no need to reimplement the +basic parsing of .proto files. However, it does mean that you need the +Google's protobuf library in order to run the generator. + +If you have downloaded a binary package for nanopb (either Windows, Linux or +Mac OS X version), the 'protoc' binary is included in the 'generator-bin' +folder. In this case, you are ready to go. Simply run this command: + + generator-bin/protoc --nanopb_out=. myprotocol.proto + +However, if you are using a git checkout or a plain source distribution, you +need to provide your own version of protoc and the Google's protobuf library. +On Linux, the necessary packages are protobuf-compiler and python-protobuf. +On Windows, you can either build Google's protobuf library from source or use +one of the binary distributions of it. In either case, if you use a separate +protoc, you need to manually give the path to nanopb generator: + + protoc --plugin=protoc-gen-nanopb=nanopb/generator/protoc-gen-nanopb ... + + + +Running the tests +================= +If you want to perform further development of the nanopb core, or to verify +its functionality using your compiler and platform, you'll want to run the +test suite. The build rules for the test suite are implemented using Scons, +so you need to have that installed. To run the tests: + + cd tests + scons + +This will show the progress of various test cases. If the output does not +end in an error, the test cases were successful. + + diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb.h b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb.h new file mode 100644 index 0000000..eea742b --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb.h @@ -0,0 +1,519 @@ +/* Common parts of the nanopb library. Most of these are quite low-level + * stuff. For the high-level interface, see pb_encode.h and pb_decode.h. + */ + +#ifndef _PB_H_ +#define _PB_H_ + +/***************************************************************** + * Nanopb compilation time options. You can change these here by * + * uncommenting the lines, or on the compiler command line. * + *****************************************************************/ + +/* Enable support for dynamically allocated fields */ +/* #define PB_ENABLE_MALLOC 1 */ + +/* Define this if your CPU architecture is big endian, i.e. it + * stores the most-significant byte first. */ +/* #define __BIG_ENDIAN__ 1 */ + +/* Increase the number of required fields that are tracked. + * A compiler warning will tell if you need this. */ +/* #define PB_MAX_REQUIRED_FIELDS 256 */ + +/* Add support for tag numbers > 255 and fields larger than 255 bytes. */ +/* #define PB_FIELD_16BIT 1 */ + +/* Add support for tag numbers > 65536 and fields larger than 65536 bytes. */ +/* #define PB_FIELD_32BIT 1 */ + +/* Disable support for error messages in order to save some code space. */ +/* #define PB_NO_ERRMSG 1 */ + +/* Disable support for custom streams (support only memory buffers). */ +/* #define PB_BUFFER_ONLY 1 */ + +/* Switch back to the old-style callback function signature. + * This was the default until nanopb-0.2.1. */ +/* #define PB_OLD_CALLBACK_STYLE */ + + +/****************************************************************** + * You usually don't need to change anything below this line. * + * Feel free to look around and use the defined macros, though. * + ******************************************************************/ + + +/* Version of the nanopb library. Just in case you want to check it in + * your own program. */ +#define NANOPB_VERSION nanopb-0.2.8 + +/* Include all the system headers needed by nanopb. You will need the + * definitions of the following: + * - strlen, memcpy, memset functions + * - [u]int8_t, [u]int16_t, [u]int32_t, [u]int64_t + * - size_t + * - bool + * + * If you don't have the standard header files, you can instead provide + * a custom header that defines or includes all this. In that case, + * define PB_SYSTEM_HEADER to the path of this file. + */ +#ifdef PB_SYSTEM_HEADER +#include PB_SYSTEM_HEADER +#else +#include +#include +#include +#include + +#ifdef PB_ENABLE_MALLOC +#include +#endif +#endif + +/* Macro for defining packed structures (compiler dependent). + * This just reduces memory requirements, but is not required. + */ +#if defined(__GNUC__) || defined(__clang__) + /* For GCC and clang */ +# define PB_PACKED_STRUCT_START +# define PB_PACKED_STRUCT_END +# define pb_packed __attribute__((packed)) +#elif defined(__ICCARM__) + /* For IAR ARM compiler */ +# define PB_PACKED_STRUCT_START _Pragma("pack(push, 1)") +# define PB_PACKED_STRUCT_END _Pragma("pack(pop)") +# define pb_packed +#elif defined(_MSC_VER) && (_MSC_VER >= 1500) + /* For Microsoft Visual C++ */ +# define PB_PACKED_STRUCT_START __pragma(pack(push, 1)) +# define PB_PACKED_STRUCT_END __pragma(pack(pop)) +# define pb_packed +#else + /* Unknown compiler */ +# define PB_PACKED_STRUCT_START +# define PB_PACKED_STRUCT_END +# define pb_packed +#endif + +/* Handly macro for suppressing unreferenced-parameter compiler warnings. */ +#ifndef UNUSED +#define UNUSED(x) (void)(x) +#endif + +/* Compile-time assertion, used for checking compatible compilation options. + * If this does not work properly on your compiler, use #define STATIC_ASSERT + * to disable it. + * + * But before doing that, check carefully the error message / place where it + * comes from to see if the error has a real cause. Unfortunately the error + * message is not always very clear to read, but you can see the reason better + * in the place where the STATIC_ASSERT macro was called. + */ +#ifndef STATIC_ASSERT +#define STATIC_ASSERT(COND,MSG) typedef char STATIC_ASSERT_MSG(MSG, __LINE__, __COUNTER__)[(COND)?1:-1]; +#define STATIC_ASSERT_MSG(MSG, LINE, COUNTER) STATIC_ASSERT_MSG_(MSG, LINE, COUNTER) +#define STATIC_ASSERT_MSG_(MSG, LINE, COUNTER) static_assertion_##MSG##LINE##COUNTER +#endif + +/* Number of required fields to keep track of. */ +#ifndef PB_MAX_REQUIRED_FIELDS +#define PB_MAX_REQUIRED_FIELDS 64 +#endif + +#if PB_MAX_REQUIRED_FIELDS < 64 +#error You should not lower PB_MAX_REQUIRED_FIELDS from the default value (64). +#endif + +/* List of possible field types. These are used in the autogenerated code. + * Least-significant 4 bits tell the scalar type + * Most-significant 4 bits specify repeated/required/packed etc. + */ + +typedef uint8_t pb_type_t; + +/**** Field data types ****/ + +/* Numeric types */ +#define PB_LTYPE_VARINT 0x00 /* int32, int64, enum, bool */ +#define PB_LTYPE_UVARINT 0x01 /* uint32, uint64 */ +#define PB_LTYPE_SVARINT 0x02 /* sint32, sint64 */ +#define PB_LTYPE_FIXED32 0x03 /* fixed32, sfixed32, float */ +#define PB_LTYPE_FIXED64 0x04 /* fixed64, sfixed64, double */ + +/* Marker for last packable field type. */ +#define PB_LTYPE_LAST_PACKABLE 0x04 + +/* Byte array with pre-allocated buffer. + * data_size is the length of the allocated PB_BYTES_ARRAY structure. */ +#define PB_LTYPE_BYTES 0x05 + +/* String with pre-allocated buffer. + * data_size is the maximum length. */ +#define PB_LTYPE_STRING 0x06 + +/* Submessage + * submsg_fields is pointer to field descriptions */ +#define PB_LTYPE_SUBMESSAGE 0x07 + +/* Extension pseudo-field + * The field contains a pointer to pb_extension_t */ +#define PB_LTYPE_EXTENSION 0x08 + +/* Number of declared LTYPES */ +#define PB_LTYPES_COUNT 9 +#define PB_LTYPE_MASK 0x0F + +/**** Field repetition rules ****/ + +#define PB_HTYPE_REQUIRED 0x00 +#define PB_HTYPE_OPTIONAL 0x10 +#define PB_HTYPE_REPEATED 0x20 +#define PB_HTYPE_MASK 0x30 + +/**** Field allocation types ****/ + +#define PB_ATYPE_STATIC 0x00 +#define PB_ATYPE_POINTER 0x80 +#define PB_ATYPE_CALLBACK 0x40 +#define PB_ATYPE_MASK 0xC0 + +#define PB_ATYPE(x) ((x) & PB_ATYPE_MASK) +#define PB_HTYPE(x) ((x) & PB_HTYPE_MASK) +#define PB_LTYPE(x) ((x) & PB_LTYPE_MASK) + +/* Data type used for storing sizes of struct fields + * and array counts. + */ +#if defined(PB_FIELD_32BIT) + typedef uint32_t pb_size_t; + typedef int32_t pb_ssize_t; +#elif defined(PB_FIELD_16BIT) + typedef uint16_t pb_size_t; + typedef int16_t pb_ssize_t; +#else + typedef uint8_t pb_size_t; + typedef int8_t pb_ssize_t; +#endif + +/* This structure is used in auto-generated constants + * to specify struct fields. + * You can change field sizes if you need structures + * larger than 256 bytes or field tags larger than 256. + * The compiler should complain if your .proto has such + * structures. Fix that by defining PB_FIELD_16BIT or + * PB_FIELD_32BIT. + */ +PB_PACKED_STRUCT_START +typedef struct _pb_field_t pb_field_t; +struct _pb_field_t { + pb_size_t tag; + pb_type_t type; + pb_size_t data_offset; /* Offset of field data, relative to previous field. */ + pb_ssize_t size_offset; /* Offset of array size or has-boolean, relative to data */ + pb_size_t data_size; /* Data size in bytes for a single item */ + pb_size_t array_size; /* Maximum number of entries in array */ + + /* Field definitions for submessage + * OR default value for all other non-array, non-callback types + * If null, then field will zeroed. */ + const void *ptr; +} pb_packed; +PB_PACKED_STRUCT_END + +/* Make sure that the standard integer types are of the expected sizes. + * All kinds of things may break otherwise.. atleast all fixed* types. + * + * If you get errors here, it probably means that your stdint.h is not + * correct for your platform. + */ +STATIC_ASSERT(sizeof(int8_t) == 1, INT8_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(uint8_t) == 1, UINT8_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(int16_t) == 2, INT16_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(uint16_t) == 2, UINT16_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(int32_t) == 4, INT32_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(uint32_t) == 4, UINT32_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(int64_t) == 8, INT64_T_WRONG_SIZE) +STATIC_ASSERT(sizeof(uint64_t) == 8, UINT64_T_WRONG_SIZE) + +/* This structure is used for 'bytes' arrays. + * It has the number of bytes in the beginning, and after that an array. + * Note that actual structs used will have a different length of bytes array. + */ +#define PB_BYTES_ARRAY_T(n) struct { size_t size; uint8_t bytes[n]; } +#define PB_BYTES_ARRAY_T_ALLOCSIZE(n) ((size_t)n + offsetof(pb_bytes_array_t, bytes)) + +struct _pb_bytes_array_t { + size_t size; + uint8_t bytes[1]; +}; +typedef struct _pb_bytes_array_t pb_bytes_array_t; + +/* This structure is used for giving the callback function. + * It is stored in the message structure and filled in by the method that + * calls pb_decode. + * + * The decoding callback will be given a limited-length stream + * If the wire type was string, the length is the length of the string. + * If the wire type was a varint/fixed32/fixed64, the length is the length + * of the actual value. + * The function may be called multiple times (especially for repeated types, + * but also otherwise if the message happens to contain the field multiple + * times.) + * + * The encoding callback will receive the actual output stream. + * It should write all the data in one call, including the field tag and + * wire type. It can write multiple fields. + * + * The callback can be null if you want to skip a field. + */ +typedef struct _pb_istream_t pb_istream_t; +typedef struct _pb_ostream_t pb_ostream_t; +typedef struct _pb_callback_t pb_callback_t; +struct _pb_callback_t { +#ifdef PB_OLD_CALLBACK_STYLE + /* Deprecated since nanopb-0.2.1 */ + union { + bool (*decode)(pb_istream_t *stream, const pb_field_t *field, void *arg); + bool (*encode)(pb_ostream_t *stream, const pb_field_t *field, const void *arg); + } funcs; +#else + /* New function signature, which allows modifying arg contents in callback. */ + union { + bool (*decode)(pb_istream_t *stream, const pb_field_t *field, void **arg); + bool (*encode)(pb_ostream_t *stream, const pb_field_t *field, void * const *arg); + } funcs; +#endif + + /* Free arg for use by callback */ + void *arg; +}; + +/* Wire types. Library user needs these only in encoder callbacks. */ +typedef enum { + PB_WT_VARINT = 0, + PB_WT_64BIT = 1, + PB_WT_STRING = 2, + PB_WT_32BIT = 5 +} pb_wire_type_t; + +/* Structure for defining the handling of unknown/extension fields. + * Usually the pb_extension_type_t structure is automatically generated, + * while the pb_extension_t structure is created by the user. However, + * if you want to catch all unknown fields, you can also create a custom + * pb_extension_type_t with your own callback. + */ +typedef struct _pb_extension_type_t pb_extension_type_t; +typedef struct _pb_extension_t pb_extension_t; +struct _pb_extension_type_t { + /* Called for each unknown field in the message. + * If you handle the field, read off all of its data and return true. + * If you do not handle the field, do not read anything and return true. + * If you run into an error, return false. + * Set to NULL for default handler. + */ + bool (*decode)(pb_istream_t *stream, pb_extension_t *extension, + uint32_t tag, pb_wire_type_t wire_type); + + /* Called once after all regular fields have been encoded. + * If you have something to write, do so and return true. + * If you do not have anything to write, just return true. + * If you run into an error, return false. + * Set to NULL for default handler. + */ + bool (*encode)(pb_ostream_t *stream, const pb_extension_t *extension); + + /* Free field for use by the callback. */ + const void *arg; +}; + +struct _pb_extension_t { + /* Type describing the extension field. Usually you'll initialize + * this to a pointer to the automatically generated structure. */ + const pb_extension_type_t *type; + + /* Destination for the decoded data. This must match the datatype + * of the extension field. */ + void *dest; + + /* Pointer to the next extension handler, or NULL. + * If this extension does not match a field, the next handler is + * automatically called. */ + pb_extension_t *next; + + /* The decoder sets this to true if the extension was found. + * Ignored for encoding. */ + bool found; +}; + +/* Memory allocation functions to use. You can define pb_realloc and + * pb_free to custom functions if you want. */ +#ifdef PB_ENABLE_MALLOC +# ifndef pb_realloc +# define pb_realloc(ptr, size) realloc(ptr, size) +# endif +# ifndef pb_free +# define pb_free(ptr) free(ptr) +# endif +#endif + +/* These macros are used to declare pb_field_t's in the constant array. */ +/* Size of a structure member, in bytes. */ +#define pb_membersize(st, m) (sizeof ((st*)0)->m) +/* Number of entries in an array. */ +#define pb_arraysize(st, m) (pb_membersize(st, m) / pb_membersize(st, m[0])) +/* Delta from start of one member to the start of another member. */ +#define pb_delta(st, m1, m2) ((int)offsetof(st, m1) - (int)offsetof(st, m2)) +/* Marks the end of the field list */ +#define PB_LAST_FIELD {0,(pb_type_t) 0,0,0,0,0,0} + +/* Macros for filling in the data_offset field */ +/* data_offset for first field in a message */ +#define PB_DATAOFFSET_FIRST(st, m1, m2) (offsetof(st, m1)) +/* data_offset for subsequent fields */ +#define PB_DATAOFFSET_OTHER(st, m1, m2) (offsetof(st, m1) - offsetof(st, m2) - pb_membersize(st, m2)) +/* Choose first/other based on m1 == m2 (deprecated, remains for backwards compatibility) */ +#define PB_DATAOFFSET_CHOOSE(st, m1, m2) (int)(offsetof(st, m1) == offsetof(st, m2) \ + ? PB_DATAOFFSET_FIRST(st, m1, m2) \ + : PB_DATAOFFSET_OTHER(st, m1, m2)) + +/* Required fields are the simplest. They just have delta (padding) from + * previous field end, and the size of the field. Pointer is used for + * submessages and default values. + */ +#define PB_REQUIRED_STATIC(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_STATIC | PB_HTYPE_REQUIRED | ltype, \ + fd, 0, pb_membersize(st, m), 0, ptr} + +/* Optional fields add the delta to the has_ variable. */ +#define PB_OPTIONAL_STATIC(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_STATIC | PB_HTYPE_OPTIONAL | ltype, \ + fd, \ + pb_delta(st, has_ ## m, m), \ + pb_membersize(st, m), 0, ptr} + +/* Repeated fields have a _count field and also the maximum number of entries. */ +#define PB_REPEATED_STATIC(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_STATIC | PB_HTYPE_REPEATED | ltype, \ + fd, \ + pb_delta(st, m ## _count, m), \ + pb_membersize(st, m[0]), \ + pb_arraysize(st, m), ptr} + +/* Allocated fields carry the size of the actual data, not the pointer */ +#define PB_REQUIRED_POINTER(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_POINTER | PB_HTYPE_REQUIRED | ltype, \ + fd, 0, pb_membersize(st, m[0]), 0, ptr} + +/* Optional fields don't need a has_ variable, as information would be redundant */ +#define PB_OPTIONAL_POINTER(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_POINTER | PB_HTYPE_OPTIONAL | ltype, \ + fd, 0, pb_membersize(st, m[0]), 0, ptr} + +/* Repeated fields have a _count field and a pointer to array of pointers */ +#define PB_REPEATED_POINTER(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_POINTER | PB_HTYPE_REPEATED | ltype, \ + fd, pb_delta(st, m ## _count, m), \ + pb_membersize(st, m[0]), 0, ptr} + +/* Callbacks are much like required fields except with special datatype. */ +#define PB_REQUIRED_CALLBACK(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_CALLBACK | PB_HTYPE_REQUIRED | ltype, \ + fd, 0, pb_membersize(st, m), 0, ptr} + +#define PB_OPTIONAL_CALLBACK(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_CALLBACK | PB_HTYPE_OPTIONAL | ltype, \ + fd, 0, pb_membersize(st, m), 0, ptr} + +#define PB_REPEATED_CALLBACK(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_CALLBACK | PB_HTYPE_REPEATED | ltype, \ + fd, 0, pb_membersize(st, m), 0, ptr} + +/* Optional extensions don't have the has_ field, as that would be redundant. */ +#define PB_OPTEXT_STATIC(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_STATIC | PB_HTYPE_OPTIONAL | ltype, \ + 0, \ + 0, \ + pb_membersize(st, m), 0, ptr} + +#define PB_OPTEXT_CALLBACK(tag, st, m, fd, ltype, ptr) \ + {tag, PB_ATYPE_CALLBACK | PB_HTYPE_OPTIONAL | ltype, \ + 0, 0, pb_membersize(st, m), 0, ptr} + +/* The mapping from protobuf types to LTYPEs is done using these macros. */ +#define PB_LTYPE_MAP_BOOL PB_LTYPE_VARINT +#define PB_LTYPE_MAP_BYTES PB_LTYPE_BYTES +#define PB_LTYPE_MAP_DOUBLE PB_LTYPE_FIXED64 +#define PB_LTYPE_MAP_ENUM PB_LTYPE_VARINT +#define PB_LTYPE_MAP_FIXED32 PB_LTYPE_FIXED32 +#define PB_LTYPE_MAP_FIXED64 PB_LTYPE_FIXED64 +#define PB_LTYPE_MAP_FLOAT PB_LTYPE_FIXED32 +#define PB_LTYPE_MAP_INT32 PB_LTYPE_VARINT +#define PB_LTYPE_MAP_INT64 PB_LTYPE_VARINT +#define PB_LTYPE_MAP_MESSAGE PB_LTYPE_SUBMESSAGE +#define PB_LTYPE_MAP_SFIXED32 PB_LTYPE_FIXED32 +#define PB_LTYPE_MAP_SFIXED64 PB_LTYPE_FIXED64 +#define PB_LTYPE_MAP_SINT32 PB_LTYPE_SVARINT +#define PB_LTYPE_MAP_SINT64 PB_LTYPE_SVARINT +#define PB_LTYPE_MAP_STRING PB_LTYPE_STRING +#define PB_LTYPE_MAP_UINT32 PB_LTYPE_UVARINT +#define PB_LTYPE_MAP_UINT64 PB_LTYPE_UVARINT +#define PB_LTYPE_MAP_EXTENSION PB_LTYPE_EXTENSION + +/* This is the actual macro used in field descriptions. + * It takes these arguments: + * - Field tag number + * - Field type: BOOL, BYTES, DOUBLE, ENUM, FIXED32, FIXED64, + * FLOAT, INT32, INT64, MESSAGE, SFIXED32, SFIXED64 + * SINT32, SINT64, STRING, UINT32, UINT64 or EXTENSION + * - Field rules: REQUIRED, OPTIONAL or REPEATED + * - Allocation: STATIC or CALLBACK + * - Message name + * - Field name + * - Previous field name (or field name again for first field) + * - Pointer to default value or submsg fields. + */ + +#define PB_FIELD(tag, type, rules, allocation, message, field, prevfield, ptr) \ + PB_ ## rules ## _ ## allocation(tag, message, field, \ + PB_DATAOFFSET_CHOOSE(message, field, prevfield), \ + PB_LTYPE_MAP_ ## type, ptr) + +/* This is a new version of the macro used by nanopb generator from + * version 0.2.3 onwards. It avoids the use of a ternary expression in + * the initialization, which confused some compilers. + * + * - Placement: FIRST or OTHER, depending on if this is the first field in structure. + * + */ +#define PB_FIELD2(tag, type, rules, allocation, placement, message, field, prevfield, ptr) \ + PB_ ## rules ## _ ## allocation(tag, message, field, \ + PB_DATAOFFSET_ ## placement(message, field, prevfield), \ + PB_LTYPE_MAP_ ## type, ptr) + + +/* These macros are used for giving out error messages. + * They are mostly a debugging aid; the main error information + * is the true/false return value from functions. + * Some code space can be saved by disabling the error + * messages if not used. + */ +#ifdef PB_NO_ERRMSG +#define PB_RETURN_ERROR(stream,msg) \ + do {\ + UNUSED(stream); \ + return false; \ + } while(0) +#define PB_GET_ERROR(stream) "(errmsg disabled)" +#else +#define PB_RETURN_ERROR(stream,msg) \ + do {\ + if ((stream)->errmsg == NULL) \ + (stream)->errmsg = (msg); \ + return false; \ + } while(0) +#define PB_GET_ERROR(stream) ((stream)->errmsg ? (stream)->errmsg : "(none)") +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.c b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.c new file mode 100644 index 0000000..90aad4a --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.c @@ -0,0 +1,1198 @@ +/* pb_decode.c -- decode a protobuf using minimal resources + * + * 2011 Petteri Aimonen + */ + +/* Use the GCC warn_unused_result attribute to check that all return values + * are propagated correctly. On other compilers and gcc before 3.4.0 just + * ignore the annotation. + */ +#if !defined(__GNUC__) || ( __GNUC__ < 3) || (__GNUC__ == 3 && __GNUC_MINOR__ < 4) + #define checkreturn +#else + #define checkreturn __attribute__((warn_unused_result)) +#endif + +#include "pb.h" +#include "pb_decode.h" + +/************************************** + * Declarations internal to this file * + **************************************/ + +/* Iterator for pb_field_t list */ +typedef struct { + const pb_field_t *start; /* Start of the pb_field_t array */ + const pb_field_t *pos; /* Current position of the iterator */ + unsigned field_index; /* Zero-based index of the field. */ + unsigned required_field_index; /* Zero-based index that counts only the required fields */ + void *dest_struct; /* Pointer to the destination structure to decode to */ + void *pData; /* Pointer where to store current field value */ + void *pSize; /* Pointer where to store the size of current array field */ +} pb_field_iterator_t; + +typedef bool (*pb_decoder_t)(pb_istream_t *stream, const pb_field_t *field, void *dest) checkreturn; + +static bool checkreturn buf_read(pb_istream_t *stream, uint8_t *buf, size_t count); +static bool checkreturn pb_decode_varint32(pb_istream_t *stream, uint32_t *dest); +static bool checkreturn read_raw_value(pb_istream_t *stream, pb_wire_type_t wire_type, uint8_t *buf, size_t *size); +static void pb_field_init(pb_field_iterator_t *iter, const pb_field_t *fields, void *dest_struct); +static bool pb_field_next(pb_field_iterator_t *iter); +static bool checkreturn pb_field_find(pb_field_iterator_t *iter, uint32_t tag); +static bool checkreturn decode_static_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter); +static bool checkreturn decode_callback_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter); +static bool checkreturn decode_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter); +static bool checkreturn default_extension_decoder(pb_istream_t *stream, pb_extension_t *extension, uint32_t tag, pb_wire_type_t wire_type); +static bool checkreturn decode_extension(pb_istream_t *stream, uint32_t tag, pb_wire_type_t wire_type, pb_field_iterator_t *iter); +static bool checkreturn find_extension_field(pb_field_iterator_t *iter); +static void pb_message_set_to_defaults(const pb_field_t fields[], void *dest_struct); +static bool checkreturn pb_dec_varint(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_uvarint(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_svarint(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_fixed32(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_fixed64(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_bytes(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_string(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_dec_submessage(pb_istream_t *stream, const pb_field_t *field, void *dest); +static bool checkreturn pb_skip_varint(pb_istream_t *stream); +static bool checkreturn pb_skip_string(pb_istream_t *stream); + +/* --- Function pointers to field decoders --- + * Order in the array must match pb_action_t LTYPE numbering. + */ +static const pb_decoder_t PB_DECODERS[PB_LTYPES_COUNT] = { + &pb_dec_varint, + &pb_dec_uvarint, + &pb_dec_svarint, + &pb_dec_fixed32, + &pb_dec_fixed64, + + &pb_dec_bytes, + &pb_dec_string, + &pb_dec_submessage, + NULL /* extensions */ +}; + +/******************************* + * pb_istream_t implementation * + *******************************/ + +static bool checkreturn buf_read(pb_istream_t *stream, uint8_t *buf, size_t count) +{ + uint8_t *source = (uint8_t*)stream->state; + stream->state = source + count; + + if (buf != NULL) + { + while (count--) + *buf++ = *source++; + } + + return true; +} + +bool checkreturn pb_read(pb_istream_t *stream, uint8_t *buf, size_t count) +{ +#ifndef PB_BUFFER_ONLY + if (buf == NULL && stream->callback != buf_read) + { + /* Skip input bytes */ + uint8_t tmp[16]; + while (count > 16) + { + if (!pb_read(stream, tmp, 16)) + return false; + + count -= 16; + } + + return pb_read(stream, tmp, count); + } +#endif + + if (stream->bytes_left < count) + PB_RETURN_ERROR(stream, "end-of-stream"); + +#ifndef PB_BUFFER_ONLY + if (!stream->callback(stream, buf, count)) + PB_RETURN_ERROR(stream, "io error"); +#else + if (!buf_read(stream, buf, count)) + return false; +#endif + + stream->bytes_left -= count; + return true; +} + +/* Read a single byte from input stream. buf may not be NULL. + * This is an optimization for the varint decoding. */ +static bool checkreturn pb_readbyte(pb_istream_t *stream, uint8_t *buf) +{ + if (stream->bytes_left == 0) + PB_RETURN_ERROR(stream, "end-of-stream"); + +#ifndef PB_BUFFER_ONLY + if (!stream->callback(stream, buf, 1)) + PB_RETURN_ERROR(stream, "io error"); +#else + *buf = *(uint8_t*)stream->state; + stream->state = (uint8_t*)stream->state + 1; +#endif + + stream->bytes_left--; + + return true; +} + +pb_istream_t pb_istream_from_buffer(uint8_t *buf, size_t bufsize) +{ + pb_istream_t stream; +#ifdef PB_BUFFER_ONLY + stream.callback = NULL; +#else + stream.callback = &buf_read; +#endif + stream.state = buf; + stream.bytes_left = bufsize; +#ifndef PB_NO_ERRMSG + stream.errmsg = NULL; +#endif + return stream; +} + +/******************** + * Helper functions * + ********************/ + +static bool checkreturn pb_decode_varint32(pb_istream_t *stream, uint32_t *dest) +{ + uint8_t byte; + uint32_t result; + + if (!pb_readbyte(stream, &byte)) + return false; + + if ((byte & 0x80) == 0) + { + /* Quick case, 1 byte value */ + result = byte; + } + else + { + /* Multibyte case */ + uint8_t bitpos = 7; + result = byte & 0x7F; + + do + { + if (bitpos >= 32) + PB_RETURN_ERROR(stream, "varint overflow"); + + if (!pb_readbyte(stream, &byte)) + return false; + + result |= (uint32_t)(byte & 0x7F) << bitpos; + bitpos = (uint8_t)(bitpos + 7); + } while (byte & 0x80); + } + + *dest = result; + return true; +} + +bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest) +{ + uint8_t byte; + uint8_t bitpos = 0; + uint64_t result = 0; + + do + { + if (bitpos >= 64) + PB_RETURN_ERROR(stream, "varint overflow"); + + if (!pb_readbyte(stream, &byte)) + return false; + + result |= (uint64_t)(byte & 0x7F) << bitpos; + bitpos = (uint8_t)(bitpos + 7); + } while (byte & 0x80); + + *dest = result; + return true; +} + +bool checkreturn pb_skip_varint(pb_istream_t *stream) +{ + uint8_t byte; + do + { + if (!pb_read(stream, &byte, 1)) + return false; + } while (byte & 0x80); + return true; +} + +bool checkreturn pb_skip_string(pb_istream_t *stream) +{ + uint32_t length; + if (!pb_decode_varint32(stream, &length)) + return false; + + return pb_read(stream, NULL, length); +} + +bool checkreturn pb_decode_tag(pb_istream_t *stream, pb_wire_type_t *wire_type, uint32_t *tag, bool *eof) +{ + uint32_t temp; + *eof = false; + *wire_type = (pb_wire_type_t) 0; + *tag = 0; + + if (!pb_decode_varint32(stream, &temp)) + { + if (stream->bytes_left == 0) + *eof = true; + + return false; + } + + if (temp == 0) + { + *eof = true; /* Special feature: allow 0-terminated messages. */ + return false; + } + + *tag = temp >> 3; + *wire_type = (pb_wire_type_t)(temp & 7); + return true; +} + +bool checkreturn pb_skip_field(pb_istream_t *stream, pb_wire_type_t wire_type) +{ + switch (wire_type) + { + case PB_WT_VARINT: return pb_skip_varint(stream); + case PB_WT_64BIT: return pb_read(stream, NULL, 8); + case PB_WT_STRING: return pb_skip_string(stream); + case PB_WT_32BIT: return pb_read(stream, NULL, 4); + default: PB_RETURN_ERROR(stream, "invalid wire_type"); + } +} + +/* Read a raw value to buffer, for the purpose of passing it to callback as + * a substream. Size is maximum size on call, and actual size on return. + */ +static bool checkreturn read_raw_value(pb_istream_t *stream, pb_wire_type_t wire_type, uint8_t *buf, size_t *size) +{ + size_t max_size = *size; + switch (wire_type) + { + case PB_WT_VARINT: + *size = 0; + do + { + (*size)++; + if (*size > max_size) return false; + if (!pb_read(stream, buf, 1)) return false; + } while (*buf++ & 0x80); + return true; + + case PB_WT_64BIT: + *size = 8; + return pb_read(stream, buf, 8); + + case PB_WT_32BIT: + *size = 4; + return pb_read(stream, buf, 4); + + default: PB_RETURN_ERROR(stream, "invalid wire_type"); + } +} + +/* Decode string length from stream and return a substream with limited length. + * Remember to close the substream using pb_close_string_substream(). + */ +bool checkreturn pb_make_string_substream(pb_istream_t *stream, pb_istream_t *substream) +{ + uint32_t size; + if (!pb_decode_varint32(stream, &size)) + return false; + + *substream = *stream; + if (substream->bytes_left < size) + PB_RETURN_ERROR(stream, "parent stream too short"); + + substream->bytes_left = size; + stream->bytes_left -= size; + return true; +} + +void pb_close_string_substream(pb_istream_t *stream, pb_istream_t *substream) +{ + stream->state = substream->state; + +#ifndef PB_NO_ERRMSG + stream->errmsg = substream->errmsg; +#endif +} + +static void pb_field_init(pb_field_iterator_t *iter, const pb_field_t *fields, void *dest_struct) +{ + iter->start = iter->pos = fields; + iter->field_index = 0; + iter->required_field_index = 0; + iter->pData = (char*)dest_struct + iter->pos->data_offset; + iter->pSize = (char*)iter->pData + iter->pos->size_offset; + iter->dest_struct = dest_struct; +} + +static bool pb_field_next(pb_field_iterator_t *iter) +{ + bool notwrapped = true; + size_t prev_size = iter->pos->data_size; + + if (PB_ATYPE(iter->pos->type) == PB_ATYPE_STATIC && + PB_HTYPE(iter->pos->type) == PB_HTYPE_REPEATED) + { + prev_size *= iter->pos->array_size; + } + else if (PB_ATYPE(iter->pos->type) == PB_ATYPE_POINTER) + { + prev_size = sizeof(void*); + } + + if (iter->pos->tag == 0) + return false; /* Only happens with empty message types */ + + if (PB_HTYPE(iter->pos->type) == PB_HTYPE_REQUIRED) + iter->required_field_index++; + + iter->pos++; + iter->field_index++; + if (iter->pos->tag == 0) + { + iter->pos = iter->start; + iter->field_index = 0; + iter->required_field_index = 0; + iter->pData = iter->dest_struct; + prev_size = 0; + notwrapped = false; + } + + iter->pData = (char*)iter->pData + prev_size + iter->pos->data_offset; + iter->pSize = (char*)iter->pData + iter->pos->size_offset; + return notwrapped; +} + +static bool checkreturn pb_field_find(pb_field_iterator_t *iter, uint32_t tag) +{ + unsigned start = iter->field_index; + + do { + if (iter->pos->tag == tag && + PB_LTYPE(iter->pos->type) != PB_LTYPE_EXTENSION) + { + return true; + } + (void)pb_field_next(iter); + } while (iter->field_index != start); + + return false; +} + +/************************* + * Decode a single field * + *************************/ + +static bool checkreturn decode_static_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter) +{ + pb_type_t type; + pb_decoder_t func; + + type = iter->pos->type; + func = PB_DECODERS[PB_LTYPE(type)]; + + switch (PB_HTYPE(type)) + { + case PB_HTYPE_REQUIRED: + return func(stream, iter->pos, iter->pData); + + case PB_HTYPE_OPTIONAL: + *(bool*)iter->pSize = true; + return func(stream, iter->pos, iter->pData); + + case PB_HTYPE_REPEATED: + if (wire_type == PB_WT_STRING + && PB_LTYPE(type) <= PB_LTYPE_LAST_PACKABLE) + { + /* Packed array */ + bool status = true; + size_t *size = (size_t*)iter->pSize; + pb_istream_t substream; + if (!pb_make_string_substream(stream, &substream)) + return false; + + while (substream.bytes_left > 0 && *size < iter->pos->array_size) + { + void *pItem = (uint8_t*)iter->pData + iter->pos->data_size * (*size); + if (!func(&substream, iter->pos, pItem)) + { + status = false; + break; + } + (*size)++; + } + pb_close_string_substream(stream, &substream); + + if (substream.bytes_left != 0) + PB_RETURN_ERROR(stream, "array overflow"); + + return status; + } + else + { + /* Repeated field */ + size_t *size = (size_t*)iter->pSize; + void *pItem = (uint8_t*)iter->pData + iter->pos->data_size * (*size); + if (*size >= iter->pos->array_size) + PB_RETURN_ERROR(stream, "array overflow"); + + (*size)++; + return func(stream, iter->pos, pItem); + } + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } +} + +#ifdef PB_ENABLE_MALLOC +/* Allocate storage for the field and store the pointer at iter->pData. + * array_size is the number of entries to reserve in an array. + */ +static bool checkreturn allocate_field(pb_istream_t *stream, void *pData, size_t data_size, size_t array_size) +{ + void *ptr = *(void**)pData; + + /* Check for multiplication overflows. */ + size_t size = 0; + if (data_size > 0 && array_size > 0) + { + /* Avoid the costly division if the sizes are small enough. + * Multiplication is safe as long as only half of bits are set + * in either multiplicand. + */ + const size_t check_limit = (size_t)1 << (sizeof(size_t) * 4); + if (data_size >= check_limit || array_size >= check_limit) + { + if (SIZE_MAX / array_size < data_size) + { + PB_RETURN_ERROR(stream, "size too large"); + } + } + + size = array_size * data_size; + } + + /* Allocate new or expand previous allocation */ + /* Note: on failure the old pointer will remain in the structure, + * the message must be freed by caller also on error return. */ + ptr = pb_realloc(ptr, size); + if (ptr == NULL) + PB_RETURN_ERROR(stream, "realloc failed"); + + *(void**)pData = ptr; + return true; +} + +/* Clear a newly allocated item in case it contains a pointer, or is a submessage. */ +static void initialize_pointer_field(void *pItem, pb_field_iterator_t *iter) +{ + if (PB_LTYPE(iter->pos->type) == PB_LTYPE_STRING || + PB_LTYPE(iter->pos->type) == PB_LTYPE_BYTES) + { + *(void**)pItem = NULL; + } + else if (PB_LTYPE(iter->pos->type) == PB_LTYPE_SUBMESSAGE) + { + pb_message_set_to_defaults((const pb_field_t *) iter->pos->ptr, pItem); + } +} +#endif + +static bool checkreturn decode_pointer_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter) +{ +#ifndef PB_ENABLE_MALLOC + UNUSED(wire_type); + UNUSED(iter); + PB_RETURN_ERROR(stream, "no malloc support"); +#else + pb_type_t type; + pb_decoder_t func; + + type = iter->pos->type; + func = PB_DECODERS[PB_LTYPE(type)]; + + switch (PB_HTYPE(type)) + { + case PB_HTYPE_REQUIRED: + case PB_HTYPE_OPTIONAL: + if (PB_LTYPE(type) == PB_LTYPE_STRING || + PB_LTYPE(type) == PB_LTYPE_BYTES) + { + return func(stream, iter->pos, iter->pData); + } + else + { + if (!allocate_field(stream, iter->pData, iter->pos->data_size, 1)) + return false; + + initialize_pointer_field(*(void**)iter->pData, iter); + return func(stream, iter->pos, *(void**)iter->pData); + } + + case PB_HTYPE_REPEATED: + if (wire_type == PB_WT_STRING + && PB_LTYPE(type) <= PB_LTYPE_LAST_PACKABLE) + { + /* Packed array, multiple items come in at once. */ + bool status = true; + size_t *size = (size_t*)iter->pSize; + size_t allocated_size = *size; + void *pItem; + pb_istream_t substream; + + if (!pb_make_string_substream(stream, &substream)) + return false; + + while (substream.bytes_left) + { + if (*size + 1 > allocated_size) + { + /* Allocate more storage. This tries to guess the + * number of remaining entries. Round the division + * upwards. */ + allocated_size += (substream.bytes_left - 1) / iter->pos->data_size + 1; + + if (!allocate_field(&substream, iter->pData, iter->pos->data_size, allocated_size)) + { + status = false; + break; + } + } + + /* Decode the array entry */ + pItem = *(uint8_t**)iter->pData + iter->pos->data_size * (*size); + initialize_pointer_field(pItem, iter); + if (!func(&substream, iter->pos, pItem)) + { + status = false; + break; + } + (*size)++; + } + pb_close_string_substream(stream, &substream); + + return status; + } + else + { + /* Normal repeated field, i.e. only one item at a time. */ + size_t *size = (size_t*)iter->pSize; + void *pItem; + + (*size)++; + if (!allocate_field(stream, iter->pData, iter->pos->data_size, *size)) + return false; + + pItem = *(uint8_t**)iter->pData + iter->pos->data_size * (*size - 1); + initialize_pointer_field(pItem, iter); + return func(stream, iter->pos, pItem); + } + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } +#endif +} + +static bool checkreturn decode_callback_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter) +{ + pb_callback_t *pCallback = (pb_callback_t*)iter->pData; + +#ifdef PB_OLD_CALLBACK_STYLE + void *arg = pCallback->arg; +#else + void **arg = &(pCallback->arg); +#endif + + if (pCallback->funcs.decode == NULL) + return pb_skip_field(stream, wire_type); + + if (wire_type == PB_WT_STRING) + { + pb_istream_t substream; + + if (!pb_make_string_substream(stream, &substream)) + return false; + + do + { + if (!pCallback->funcs.decode(&substream, iter->pos, arg)) + PB_RETURN_ERROR(stream, "callback failed"); + } while (substream.bytes_left); + + pb_close_string_substream(stream, &substream); + return true; + } + else + { + /* Copy the single scalar value to stack. + * This is required so that we can limit the stream length, + * which in turn allows to use same callback for packed and + * not-packed fields. */ + pb_istream_t substream; + uint8_t buffer[10]; + size_t size = sizeof(buffer); + + if (!read_raw_value(stream, wire_type, buffer, &size)) + return false; + substream = pb_istream_from_buffer(buffer, size); + + return pCallback->funcs.decode(&substream, iter->pos, arg); + } +} + +static bool checkreturn decode_field(pb_istream_t *stream, pb_wire_type_t wire_type, pb_field_iterator_t *iter) +{ + switch (PB_ATYPE(iter->pos->type)) + { + case PB_ATYPE_STATIC: + return decode_static_field(stream, wire_type, iter); + + case PB_ATYPE_POINTER: + return decode_pointer_field(stream, wire_type, iter); + + case PB_ATYPE_CALLBACK: + return decode_callback_field(stream, wire_type, iter); + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } +} + +/* Default handler for extension fields. Expects a pb_field_t structure + * in extension->type->arg. */ +static bool checkreturn default_extension_decoder(pb_istream_t *stream, + pb_extension_t *extension, uint32_t tag, pb_wire_type_t wire_type) +{ + const pb_field_t *field = (const pb_field_t*)extension->type->arg; + pb_field_iterator_t iter; + + if (field->tag != tag) + return true; + + iter.start = field; + iter.pos = field; + iter.field_index = 0; + iter.required_field_index = 0; + iter.dest_struct = extension->dest; + iter.pData = extension->dest; + iter.pSize = &extension->found; + + return decode_field(stream, wire_type, &iter); +} + +/* Try to decode an unknown field as an extension field. Tries each extension + * decoder in turn, until one of them handles the field or loop ends. */ +static bool checkreturn decode_extension(pb_istream_t *stream, + uint32_t tag, pb_wire_type_t wire_type, pb_field_iterator_t *iter) +{ + pb_extension_t *extension = *(pb_extension_t* const *)iter->pData; + size_t pos = stream->bytes_left; + + while (extension != NULL && pos == stream->bytes_left) + { + bool status; + if (extension->type->decode) + status = extension->type->decode(stream, extension, tag, wire_type); + else + status = default_extension_decoder(stream, extension, tag, wire_type); + + if (!status) + return false; + + extension = extension->next; + } + + return true; +} + +/* Step through the iterator until an extension field is found or until all + * entries have been checked. There can be only one extension field per + * message. Returns false if no extension field is found. */ +static bool checkreturn find_extension_field(pb_field_iterator_t *iter) +{ + unsigned start = iter->field_index; + + do { + if (PB_LTYPE(iter->pos->type) == PB_LTYPE_EXTENSION) + return true; + (void)pb_field_next(iter); + } while (iter->field_index != start); + + return false; +} + +/* Initialize message fields to default values, recursively */ +static void pb_message_set_to_defaults(const pb_field_t fields[], void *dest_struct) +{ + pb_field_iterator_t iter; + pb_field_init(&iter, fields, dest_struct); + + do + { + pb_type_t type; + type = iter.pos->type; + + /* Avoid crash on empty message types (zero fields) */ + if (iter.pos->tag == 0) + continue; + + if (PB_ATYPE(type) == PB_ATYPE_STATIC) + { + if (PB_HTYPE(type) == PB_HTYPE_OPTIONAL) + { + /* Set has_field to false. Still initialize the optional field + * itself also. */ + *(bool*)iter.pSize = false; + } + else if (PB_HTYPE(type) == PB_HTYPE_REPEATED) + { + /* Set array count to 0, no need to initialize contents. */ + *(size_t*)iter.pSize = 0; + continue; + } + + if (PB_LTYPE(iter.pos->type) == PB_LTYPE_SUBMESSAGE) + { + /* Initialize submessage to defaults */ + pb_message_set_to_defaults((const pb_field_t *) iter.pos->ptr, iter.pData); + } + else if (iter.pos->ptr != NULL) + { + /* Initialize to default value */ + memcpy(iter.pData, iter.pos->ptr, iter.pos->data_size); + } + else + { + /* Initialize to zeros */ + memset(iter.pData, 0, iter.pos->data_size); + } + } + else if (PB_ATYPE(type) == PB_ATYPE_POINTER) + { + /* Initialize the pointer to NULL. */ + *(void**)iter.pData = NULL; + + /* Initialize array count to 0. */ + if (PB_HTYPE(type) == PB_HTYPE_REPEATED) + { + *(size_t*)iter.pSize = 0; + } + } + else if (PB_ATYPE(type) == PB_ATYPE_CALLBACK) + { + /* Don't overwrite callback */ + } + } while (pb_field_next(&iter)); +} + +/********************* + * Decode all fields * + *********************/ + +bool checkreturn pb_decode_noinit(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct) +{ + uint8_t fields_seen[(PB_MAX_REQUIRED_FIELDS + 7) / 8] = {0, 0, 0, 0, 0, 0, 0, 0}; + uint32_t extension_range_start = 0; + pb_field_iterator_t iter; + + pb_field_init(&iter, fields, dest_struct); + + while (stream->bytes_left) + { + uint32_t tag; + pb_wire_type_t wire_type; + bool eof; + + if (!pb_decode_tag(stream, &wire_type, &tag, &eof)) + { + if (eof) + break; + else + return false; + } + + if (!pb_field_find(&iter, tag)) + { + /* No match found, check if it matches an extension. */ + if (tag >= extension_range_start) + { + if (!find_extension_field(&iter)) + extension_range_start = (uint32_t)-1; + else + extension_range_start = iter.pos->tag; + + if (tag >= extension_range_start) + { + size_t pos = stream->bytes_left; + + if (!decode_extension(stream, tag, wire_type, &iter)) + return false; + + if (pos != stream->bytes_left) + { + /* The field was handled */ + continue; + } + } + } + + /* No match found, skip data */ + if (!pb_skip_field(stream, wire_type)) + return false; + continue; + } + + if (PB_HTYPE(iter.pos->type) == PB_HTYPE_REQUIRED + && iter.required_field_index < PB_MAX_REQUIRED_FIELDS) + { + fields_seen[iter.required_field_index >> 3] |= (uint8_t)(1 << (iter.required_field_index & 7)); + } + + if (!decode_field(stream, wire_type, &iter)) + return false; + } + + /* Check that all required fields were present. */ + { + /* First figure out the number of required fields by + * seeking to the end of the field array. Usually we + * are already close to end after decoding. + */ + unsigned req_field_count; + pb_type_t last_type; + unsigned i; + do { + req_field_count = iter.required_field_index; + last_type = iter.pos->type; + } while (pb_field_next(&iter)); + + /* Fixup if last field was also required. */ + if (PB_HTYPE(last_type) == PB_HTYPE_REQUIRED && iter.pos->tag != 0) + req_field_count++; + + /* Check the whole bytes */ + for (i = 0; i < (req_field_count >> 3); i++) + { + if (fields_seen[i] != 0xFF) + PB_RETURN_ERROR(stream, "missing required field"); + } + + /* Check the remaining bits */ + if (fields_seen[req_field_count >> 3] != (0xFF >> (8 - (req_field_count & 7)))) + PB_RETURN_ERROR(stream, "missing required field"); + } + + return true; +} + +bool checkreturn pb_decode(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct) +{ + bool status; + pb_message_set_to_defaults(fields, dest_struct); + status = pb_decode_noinit(stream, fields, dest_struct); + +#ifdef PB_ENABLE_MALLOC + if (!status) + pb_release(fields, dest_struct); +#endif + + return status; +} + +bool pb_decode_delimited(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct) +{ + pb_istream_t substream; + bool status; + + if (!pb_make_string_substream(stream, &substream)) + return false; + + status = pb_decode(&substream, fields, dest_struct); + pb_close_string_substream(stream, &substream); + return status; +} + +#ifdef PB_ENABLE_MALLOC +void pb_release(const pb_field_t fields[], void *dest_struct) +{ + pb_field_iterator_t iter; + pb_field_init(&iter, fields, dest_struct); + + do + { + pb_type_t type; + type = iter.pos->type; + + /* Avoid crash on empty message types (zero fields) */ + if (iter.pos->tag == 0) + continue; + + if (PB_ATYPE(type) == PB_ATYPE_POINTER) + { + if (PB_HTYPE(type) == PB_HTYPE_REPEATED && + (PB_LTYPE(type) == PB_LTYPE_STRING || + PB_LTYPE(type) == PB_LTYPE_BYTES)) + { + /* Release entries in repeated string or bytes array */ + void **pItem = *(void***)iter.pData; + size_t count = *(size_t*)iter.pSize; + while (count--) + { + pb_free(*pItem); + *pItem++ = NULL; + } + } + else if (PB_LTYPE(type) == PB_LTYPE_SUBMESSAGE) + { + /* Release fields in submessages */ + void *pItem = *(void**)iter.pData; + size_t count = (pItem ? 1 : 0); + + if (PB_HTYPE(type) == PB_HTYPE_REPEATED) + { + count = *(size_t*)iter.pSize; + } + + while (count--) + { + pb_release((const pb_field_t*)iter.pos->ptr, pItem); + pItem = (uint8_t*)pItem + iter.pos->data_size; + } + } + + /* Release main item */ + pb_free(*(void**)iter.pData); + *(void**)iter.pData = NULL; + } + } while (pb_field_next(&iter)); +} +#endif + +/* Field decoders */ + +bool pb_decode_svarint(pb_istream_t *stream, int64_t *dest) +{ + uint64_t value; + if (!pb_decode_varint(stream, &value)) + return false; + + if (value & 1) + *dest = (int64_t)(~(value >> 1)); + else + *dest = (int64_t)(value >> 1); + + return true; +} + +bool pb_decode_fixed32(pb_istream_t *stream, void *dest) +{ + #ifdef __BIG_ENDIAN__ + uint8_t *bytes = (uint8_t*)dest; + uint8_t lebytes[4]; + + if (!pb_read(stream, lebytes, 4)) + return false; + + bytes[0] = lebytes[3]; + bytes[1] = lebytes[2]; + bytes[2] = lebytes[1]; + bytes[3] = lebytes[0]; + return true; + #else + return pb_read(stream, (uint8_t*)dest, 4); + #endif +} + +bool pb_decode_fixed64(pb_istream_t *stream, void *dest) +{ + #ifdef __BIG_ENDIAN__ + uint8_t *bytes = (uint8_t*)dest; + uint8_t lebytes[8]; + + if (!pb_read(stream, lebytes, 8)) + return false; + + bytes[0] = lebytes[7]; + bytes[1] = lebytes[6]; + bytes[2] = lebytes[5]; + bytes[3] = lebytes[4]; + bytes[4] = lebytes[3]; + bytes[5] = lebytes[2]; + bytes[6] = lebytes[1]; + bytes[7] = lebytes[0]; + return true; + #else + return pb_read(stream, (uint8_t*)dest, 8); + #endif +} + +static bool checkreturn pb_dec_varint(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + uint64_t value; + if (!pb_decode_varint(stream, &value)) + return false; + + switch (field->data_size) + { + case 1: *(int8_t*)dest = (int8_t)value; break; + case 2: *(int16_t*)dest = (int16_t)value; break; + case 4: *(int32_t*)dest = (int32_t)value; break; + case 8: *(int64_t*)dest = (int64_t)value; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return true; +} + +static bool checkreturn pb_dec_uvarint(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + uint64_t value; + if (!pb_decode_varint(stream, &value)) + return false; + + switch (field->data_size) + { + case 4: *(uint32_t*)dest = (uint32_t)value; break; + case 8: *(uint64_t*)dest = value; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return true; +} + +static bool checkreturn pb_dec_svarint(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + int64_t value; + if (!pb_decode_svarint(stream, &value)) + return false; + + switch (field->data_size) + { + case 4: *(int32_t*)dest = (int32_t)value; break; + case 8: *(int64_t*)dest = value; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return true; +} + +static bool checkreturn pb_dec_fixed32(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + UNUSED(field); + return pb_decode_fixed32(stream, dest); +} + +static bool checkreturn pb_dec_fixed64(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + UNUSED(field); + return pb_decode_fixed64(stream, dest); +} + +static bool checkreturn pb_dec_bytes(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + uint32_t size; + pb_bytes_array_t *bdest; + + if (!pb_decode_varint32(stream, &size)) + return false; + + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER) + { +#ifndef PB_ENABLE_MALLOC + PB_RETURN_ERROR(stream, "no malloc support"); +#else + if (!allocate_field(stream, dest, PB_BYTES_ARRAY_T_ALLOCSIZE(size), 1)) + return false; + bdest = *(pb_bytes_array_t**)dest; +#endif + } + else + { + if (PB_BYTES_ARRAY_T_ALLOCSIZE(size) > field->data_size) + PB_RETURN_ERROR(stream, "bytes overflow"); + bdest = (pb_bytes_array_t*)dest; + } + + bdest->size = size; + return pb_read(stream, bdest->bytes, size); +} + +static bool checkreturn pb_dec_string(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + uint32_t size; + size_t alloc_size; + bool status; + if (!pb_decode_varint32(stream, &size)) + return false; + + /* Space for null terminator */ + alloc_size = size + 1; + + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER) + { +#ifndef PB_ENABLE_MALLOC + PB_RETURN_ERROR(stream, "no malloc support"); +#else + if (!allocate_field(stream, dest, alloc_size, 1)) + return false; + dest = *(void**)dest; +#endif + } + else + { + if (alloc_size > field->data_size) + PB_RETURN_ERROR(stream, "string overflow"); + } + + status = pb_read(stream, (uint8_t*)dest, size); + *((uint8_t*)dest + size) = 0; + return status; +} + +static bool checkreturn pb_dec_submessage(pb_istream_t *stream, const pb_field_t *field, void *dest) +{ + bool status; + pb_istream_t substream; + const pb_field_t* submsg_fields = (const pb_field_t*)field->ptr; + + if (!pb_make_string_substream(stream, &substream)) + return false; + + if (field->ptr == NULL) + PB_RETURN_ERROR(stream, "invalid field descriptor"); + + /* New array entries need to be initialized, while required and optional + * submessages have already been initialized in the top-level pb_decode. */ + if (PB_HTYPE(field->type) == PB_HTYPE_REPEATED) + status = pb_decode(&substream, submsg_fields, dest); + else + status = pb_decode_noinit(&substream, submsg_fields, dest); + + pb_close_string_substream(stream, &substream); + return status; +} diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.h b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.h new file mode 100644 index 0000000..0e000f6 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_decode.h @@ -0,0 +1,149 @@ +/* pb_decode.h: Functions to decode protocol buffers. Depends on pb_decode.c. + * The main function is pb_decode. You also need an input stream, and the + * field descriptions created by nanopb_generator.py. + */ + +#ifndef _PB_DECODE_H_ +#define _PB_DECODE_H_ + +#include "pb.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Structure for defining custom input streams. You will need to provide + * a callback function to read the bytes from your storage, which can be + * for example a file or a network socket. + * + * The callback must conform to these rules: + * + * 1) Return false on IO errors. This will cause decoding to abort. + * 2) You can use state to store your own data (e.g. buffer pointer), + * and rely on pb_read to verify that no-body reads past bytes_left. + * 3) Your callback may be used with substreams, in which case bytes_left + * is different than from the main stream. Don't use bytes_left to compute + * any pointers. + */ +struct _pb_istream_t +{ +#ifdef PB_BUFFER_ONLY + /* Callback pointer is not used in buffer-only configuration. + * Having an int pointer here allows binary compatibility but + * gives an error if someone tries to assign callback function. + */ + int *callback; +#else + bool (*callback)(pb_istream_t *stream, uint8_t *buf, size_t count); +#endif + + void *state; /* Free field for use by callback implementation */ + size_t bytes_left; + +#ifndef PB_NO_ERRMSG + const char *errmsg; +#endif +}; + +/*************************** + * Main decoding functions * + ***************************/ + +/* Decode a single protocol buffers message from input stream into a C structure. + * Returns true on success, false on any failure. + * The actual struct pointed to by dest must match the description in fields. + * Callback fields of the destination structure must be initialized by caller. + * All other fields will be initialized by this function. + * + * Example usage: + * MyMessage msg = {}; + * uint8_t buffer[64]; + * pb_istream_t stream; + * + * // ... read some data into buffer ... + * + * stream = pb_istream_from_buffer(buffer, count); + * pb_decode(&stream, MyMessage_fields, &msg); + */ +bool pb_decode(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct); + +/* Same as pb_decode, except does not initialize the destination structure + * to default values. This is slightly faster if you need no default values + * and just do memset(struct, 0, sizeof(struct)) yourself. + * + * This can also be used for 'merging' two messages, i.e. update only the + * fields that exist in the new message. + * + * Note: If this function returns with an error, it will not release any + * dynamically allocated fields. You will need to call pb_release() yourself. + */ +bool pb_decode_noinit(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct); + +/* Same as pb_decode, except expects the stream to start with the message size + * encoded as varint. Corresponds to parseDelimitedFrom() in Google's + * protobuf API. + */ +bool pb_decode_delimited(pb_istream_t *stream, const pb_field_t fields[], void *dest_struct); + +#ifdef PB_ENABLE_MALLOC +/* Release any allocated pointer fields. If you use dynamic allocation, you should + * call this for any successfully decoded message when you are done with it. If + * pb_decode() returns with an error, the message is already released. + */ +void pb_release(const pb_field_t fields[], void *dest_struct); +#endif + + +/************************************** + * Functions for manipulating streams * + **************************************/ + +/* Create an input stream for reading from a memory buffer. + * + * Alternatively, you can use a custom stream that reads directly from e.g. + * a file or a network socket. + */ +pb_istream_t pb_istream_from_buffer(uint8_t *buf, size_t bufsize); + +/* Function to read from a pb_istream_t. You can use this if you need to + * read some custom header data, or to read data in field callbacks. + */ +bool pb_read(pb_istream_t *stream, uint8_t *buf, size_t count); + + +/************************************************ + * Helper functions for writing field callbacks * + ************************************************/ + +/* Decode the tag for the next field in the stream. Gives the wire type and + * field tag. At end of the message, returns false and sets eof to true. */ +bool pb_decode_tag(pb_istream_t *stream, pb_wire_type_t *wire_type, uint32_t *tag, bool *eof); + +/* Skip the field payload data, given the wire type. */ +bool pb_skip_field(pb_istream_t *stream, pb_wire_type_t wire_type); + +/* Decode an integer in the varint format. This works for bool, enum, int32, + * int64, uint32 and uint64 field types. */ +bool pb_decode_varint(pb_istream_t *stream, uint64_t *dest); + +/* Decode an integer in the zig-zagged svarint format. This works for sint32 + * and sint64. */ +bool pb_decode_svarint(pb_istream_t *stream, int64_t *dest); + +/* Decode a fixed32, sfixed32 or float value. You need to pass a pointer to + * a 4-byte wide C variable. */ +bool pb_decode_fixed32(pb_istream_t *stream, void *dest); + +/* Decode a fixed64, sfixed64 or double value. You need to pass a pointer to + * a 8-byte wide C variable. */ +bool pb_decode_fixed64(pb_istream_t *stream, void *dest); + +/* Make a limited-length substream for reading a PB_WT_STRING field. */ +bool pb_make_string_substream(pb_istream_t *stream, pb_istream_t *substream); +void pb_close_string_substream(pb_istream_t *stream, pb_istream_t *substream); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.c b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.c new file mode 100644 index 0000000..c3e0b25 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.c @@ -0,0 +1,671 @@ +/* pb_encode.c -- encode a protobuf using minimal resources + * + * 2011 Petteri Aimonen + */ + +#include "pb.h" +#include "pb_encode.h" + +/* Use the GCC warn_unused_result attribute to check that all return values + * are propagated correctly. On other compilers and gcc before 3.4.0 just + * ignore the annotation. + */ +#if !defined(__GNUC__) || ( __GNUC__ < 3) || (__GNUC__ == 3 && __GNUC_MINOR__ < 4) + #define checkreturn +#else + #define checkreturn __attribute__((warn_unused_result)) +#endif + +/************************************** + * Declarations internal to this file * + **************************************/ +typedef bool (*pb_encoder_t)(pb_ostream_t *stream, const pb_field_t *field, const void *src) checkreturn; + +static bool checkreturn buf_write(pb_ostream_t *stream, const uint8_t *buf, size_t count); +static bool checkreturn encode_array(pb_ostream_t *stream, const pb_field_t *field, const void *pData, size_t count, pb_encoder_t func); +static bool checkreturn encode_field(pb_ostream_t *stream, const pb_field_t *field, const void *pData); +static bool checkreturn default_extension_encoder(pb_ostream_t *stream, const pb_extension_t *extension); +static bool checkreturn encode_extension_field(pb_ostream_t *stream, const pb_field_t *field, const void *pData); +static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_uvarint(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_svarint(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_fixed32(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_fixed64(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_t *field, const void *src); +static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_t *field, const void *src); + +/* --- Function pointers to field encoders --- + * Order in the array must match pb_action_t LTYPE numbering. + */ +static const pb_encoder_t PB_ENCODERS[PB_LTYPES_COUNT] = { + &pb_enc_varint, + &pb_enc_uvarint, + &pb_enc_svarint, + &pb_enc_fixed32, + &pb_enc_fixed64, + + &pb_enc_bytes, + &pb_enc_string, + &pb_enc_submessage, + NULL /* extensions */ +}; + +/******************************* + * pb_ostream_t implementation * + *******************************/ + +static bool checkreturn buf_write(pb_ostream_t *stream, const uint8_t *buf, size_t count) +{ + uint8_t *dest = (uint8_t*)stream->state; + stream->state = dest + count; + + while (count--) + *dest++ = *buf++; + + return true; +} + +pb_ostream_t pb_ostream_from_buffer(uint8_t *buf, size_t bufsize) +{ + pb_ostream_t stream; +#ifdef PB_BUFFER_ONLY + stream.callback = (void*)1; /* Just a marker value */ +#else + stream.callback = &buf_write; +#endif + stream.state = buf; + stream.max_size = bufsize; + stream.bytes_written = 0; +#ifndef PB_NO_ERRMSG + stream.errmsg = NULL; +#endif + return stream; +} + +bool checkreturn pb_write(pb_ostream_t *stream, const uint8_t *buf, size_t count) +{ + if (stream->callback != NULL) + { + if (stream->bytes_written + count > stream->max_size) + PB_RETURN_ERROR(stream, "stream full"); + +#ifdef PB_BUFFER_ONLY + if (!buf_write(stream, buf, count)) + PB_RETURN_ERROR(stream, "io error"); +#else + if (!stream->callback(stream, buf, count)) + PB_RETURN_ERROR(stream, "io error"); +#endif + } + + stream->bytes_written += count; + return true; +} + +/************************* + * Encode a single field * + *************************/ + +/* Encode a static array. Handles the size calculations and possible packing. */ +static bool checkreturn encode_array(pb_ostream_t *stream, const pb_field_t *field, + const void *pData, size_t count, pb_encoder_t func) +{ + size_t i; + const void *p; + size_t size; + + if (count == 0) + return true; + + if (PB_ATYPE(field->type) != PB_ATYPE_POINTER && count > field->array_size) + PB_RETURN_ERROR(stream, "array max size exceeded"); + + /* We always pack arrays if the datatype allows it. */ + if (PB_LTYPE(field->type) <= PB_LTYPE_LAST_PACKABLE) + { + if (!pb_encode_tag(stream, PB_WT_STRING, field->tag)) + return false; + + /* Determine the total size of packed array. */ + if (PB_LTYPE(field->type) == PB_LTYPE_FIXED32) + { + size = 4 * count; + } + else if (PB_LTYPE(field->type) == PB_LTYPE_FIXED64) + { + size = 8 * count; + } + else + { + pb_ostream_t sizestream = PB_OSTREAM_SIZING; + p = pData; + for (i = 0; i < count; i++) + { + if (!func(&sizestream, field, p)) + return false; + p = (const char*)p + field->data_size; + } + size = sizestream.bytes_written; + } + + if (!pb_encode_varint(stream, (uint64_t)size)) + return false; + + if (stream->callback == NULL) + return pb_write(stream, NULL, size); /* Just sizing.. */ + + /* Write the data */ + p = pData; + for (i = 0; i < count; i++) + { + if (!func(stream, field, p)) + return false; + p = (const char*)p + field->data_size; + } + } + else + { + p = pData; + for (i = 0; i < count; i++) + { + if (!pb_encode_tag_for_field(stream, field)) + return false; + + /* Normally the data is stored directly in the array entries, but + * for pointer-type string and bytes fields, the array entries are + * actually pointers themselves also. So we have to dereference once + * more to get to the actual data. */ + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER && + (PB_LTYPE(field->type) == PB_LTYPE_STRING || + PB_LTYPE(field->type) == PB_LTYPE_BYTES)) + { + if (!func(stream, field, *(const void* const*)p)) + return false; + } + else + { + if (!func(stream, field, p)) + return false; + } + p = (const char*)p + field->data_size; + } + } + + return true; +} + +/* Encode a field with static or pointer allocation, i.e. one whose data + * is available to the encoder directly. */ +static bool checkreturn encode_basic_field(pb_ostream_t *stream, + const pb_field_t *field, const void *pData) +{ + pb_encoder_t func; + const void *pSize; + bool implicit_has = true; + + func = PB_ENCODERS[PB_LTYPE(field->type)]; + + if (field->size_offset) + pSize = (const char*)pData + field->size_offset; + else + pSize = &implicit_has; + + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER) + { + /* pData is a pointer to the field, which contains pointer to + * the data. If the 2nd pointer is NULL, it is interpreted as if + * the has_field was false. + */ + + pData = *(const void* const*)pData; + implicit_has = (pData != NULL); + } + + switch (PB_HTYPE(field->type)) + { + case PB_HTYPE_REQUIRED: + if (!pData) + PB_RETURN_ERROR(stream, "missing required field"); + if (!pb_encode_tag_for_field(stream, field)) + return false; + if (!func(stream, field, pData)) + return false; + break; + + case PB_HTYPE_OPTIONAL: + /* + * KUKA adjustment for VxWorks + */ + + if (*(const char*)pSize) + { + if (!pb_encode_tag_for_field(stream, field)) + return false; + + if (!func(stream, field, pData)) + return false; + } + break; + + case PB_HTYPE_REPEATED: + if (!encode_array(stream, field, pData, *(const size_t*)pSize, func)) + return false; + break; + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } + + return true; +} + +/* Encode a field with callback semantics. This means that a user function is + * called to provide and encode the actual data. */ +static bool checkreturn encode_callback_field(pb_ostream_t *stream, + const pb_field_t *field, const void *pData) +{ + const pb_callback_t *callback = (const pb_callback_t*)pData; + +#ifdef PB_OLD_CALLBACK_STYLE + const void *arg = callback->arg; +#else + void * const *arg = &(callback->arg); +#endif + + if (callback->funcs.encode != NULL) + { + if (!callback->funcs.encode(stream, field, arg)) + PB_RETURN_ERROR(stream, "callback error"); + } + return true; +} + +/* Encode a single field of any callback or static type. */ +static bool checkreturn encode_field(pb_ostream_t *stream, + const pb_field_t *field, const void *pData) +{ + switch (PB_ATYPE(field->type)) + { + case PB_ATYPE_STATIC: + case PB_ATYPE_POINTER: + return encode_basic_field(stream, field, pData); + + case PB_ATYPE_CALLBACK: + return encode_callback_field(stream, field, pData); + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } +} + +/* Default handler for extension fields. Expects to have a pb_field_t + * pointer in the extension->type->arg field. */ +static bool checkreturn default_extension_encoder(pb_ostream_t *stream, + const pb_extension_t *extension) +{ + const pb_field_t *field = (const pb_field_t*)extension->type->arg; + return encode_field(stream, field, extension->dest); +} + +/* Walk through all the registered extensions and give them a chance + * to encode themselves. */ +static bool checkreturn encode_extension_field(pb_ostream_t *stream, + const pb_field_t *field, const void *pData) +{ + const pb_extension_t *extension = *(const pb_extension_t* const *)pData; + UNUSED(field); + + while (extension) + { + bool status; + if (extension->type->encode) + status = extension->type->encode(stream, extension); + else + status = default_extension_encoder(stream, extension); + + if (!status) + return false; + + extension = extension->next; + } + + return true; +} + +/********************* + * Encode all fields * + *********************/ + +bool checkreturn pb_encode(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct) +{ + const pb_field_t *field = fields; + const void *pData = src_struct; + size_t prev_size = 0; + + while (field->tag != 0) + { + pData = (const char*)pData + prev_size + field->data_offset; + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER) + prev_size = sizeof(const void*); + else + prev_size = field->data_size; + + /* Special case for static arrays */ + if (PB_ATYPE(field->type) == PB_ATYPE_STATIC && + PB_HTYPE(field->type) == PB_HTYPE_REPEATED) + { + prev_size *= field->array_size; + } + + if (PB_LTYPE(field->type) == PB_LTYPE_EXTENSION) + { + /* Special case for the extension field placeholder */ + if (!encode_extension_field(stream, field, pData)) + return false; + } + else + { + /* Regular field */ + if (!encode_field(stream, field, pData)) + return false; + } + + field++; + } + + return true; +} + +bool pb_encode_delimited(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct) +{ + return pb_encode_submessage(stream, fields, src_struct); +} + +bool pb_get_encoded_size(size_t *size, const pb_field_t fields[], const void *src_struct) +{ + pb_ostream_t stream = PB_OSTREAM_SIZING; + + if (!pb_encode(&stream, fields, src_struct)) + return false; + + *size = stream.bytes_written; + return true; +} + +/******************** + * Helper functions * + ********************/ +bool checkreturn pb_encode_varint(pb_ostream_t *stream, uint64_t value) +{ + uint8_t buffer[10]; + size_t i = 0; + + if (value == 0) + return pb_write(stream, (uint8_t*)&value, 1); + + while (value) + { + buffer[i] = (uint8_t)((value & 0x7F) | 0x80); + value >>= 7; + i++; + } + buffer[i-1] &= 0x7F; /* Unset top bit on last byte */ + + return pb_write(stream, buffer, i); +} + +bool checkreturn pb_encode_svarint(pb_ostream_t *stream, int64_t value) +{ + uint64_t zigzagged; + if (value < 0) + zigzagged = ~((uint64_t)value << 1); + else + zigzagged = (uint64_t)value << 1; + + return pb_encode_varint(stream, zigzagged); +} + +bool checkreturn pb_encode_fixed32(pb_ostream_t *stream, const void *value) +{ + #ifdef __BIG_ENDIAN__ + const uint8_t *bytes = value; + uint8_t lebytes[4]; + lebytes[0] = bytes[3]; + lebytes[1] = bytes[2]; + lebytes[2] = bytes[1]; + lebytes[3] = bytes[0]; + return pb_write(stream, lebytes, 4); + #else + return pb_write(stream, (const uint8_t*)value, 4); + #endif +} + +bool checkreturn pb_encode_fixed64(pb_ostream_t *stream, const void *value) +{ + #ifdef __BIG_ENDIAN__ + const uint8_t *bytes = value; + uint8_t lebytes[8]; + lebytes[0] = bytes[7]; + lebytes[1] = bytes[6]; + lebytes[2] = bytes[5]; + lebytes[3] = bytes[4]; + lebytes[4] = bytes[3]; + lebytes[5] = bytes[2]; + lebytes[6] = bytes[1]; + lebytes[7] = bytes[0]; + return pb_write(stream, lebytes, 8); + #else + return pb_write(stream, (const uint8_t*)value, 8); + #endif +} + +bool checkreturn pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number) +{ + uint64_t tag = ((uint64_t)field_number << 3) | wiretype; + return pb_encode_varint(stream, tag); +} + +bool checkreturn pb_encode_tag_for_field(pb_ostream_t *stream, const pb_field_t *field) +{ + pb_wire_type_t wiretype; + switch (PB_LTYPE(field->type)) + { + case PB_LTYPE_VARINT: + case PB_LTYPE_UVARINT: + case PB_LTYPE_SVARINT: + wiretype = PB_WT_VARINT; + break; + + case PB_LTYPE_FIXED32: + wiretype = PB_WT_32BIT; + break; + + case PB_LTYPE_FIXED64: + wiretype = PB_WT_64BIT; + break; + + case PB_LTYPE_BYTES: + case PB_LTYPE_STRING: + case PB_LTYPE_SUBMESSAGE: + wiretype = PB_WT_STRING; + break; + + default: + PB_RETURN_ERROR(stream, "invalid field type"); + } + + return pb_encode_tag(stream, wiretype, field->tag); +} + +bool checkreturn pb_encode_string(pb_ostream_t *stream, const uint8_t *buffer, size_t size) +{ + if (!pb_encode_varint(stream, (uint64_t)size)) + return false; + + return pb_write(stream, buffer, size); +} + +bool checkreturn pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct) +{ + /* First calculate the message size using a non-writing substream. */ + pb_ostream_t substream = PB_OSTREAM_SIZING; + size_t size; + bool status; + + if (!pb_encode(&substream, fields, src_struct)) + { +#ifndef PB_NO_ERRMSG + stream->errmsg = substream.errmsg; +#endif + return false; + } + + size = substream.bytes_written; + + if (!pb_encode_varint(stream, (uint64_t)size)) + return false; + + if (stream->callback == NULL) + return pb_write(stream, NULL, size); /* Just sizing */ + + if (stream->bytes_written + size > stream->max_size) + PB_RETURN_ERROR(stream, "stream full"); + + /* Use a substream to verify that a callback doesn't write more than + * what it did the first time. */ + substream.callback = stream->callback; + substream.state = stream->state; + substream.max_size = size; + substream.bytes_written = 0; +#ifndef PB_NO_ERRMSG + substream.errmsg = NULL; +#endif + + status = pb_encode(&substream, fields, src_struct); + + stream->bytes_written += substream.bytes_written; + stream->state = substream.state; +#ifndef PB_NO_ERRMSG + stream->errmsg = substream.errmsg; +#endif + + if (substream.bytes_written != size) + PB_RETURN_ERROR(stream, "submsg size changed"); + + return status; +} + +/* Field encoders */ + +static bool checkreturn pb_enc_varint(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + int64_t value = 0; + + /* Cases 1 and 2 are for compilers that have smaller types for bool + * or enums. */ + switch (field->data_size) + { + case 1: value = *(const int8_t*)src; break; + case 2: value = *(const int16_t*)src; break; + case 4: value = *(const int32_t*)src; break; + case 8: value = *(const int64_t*)src; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return pb_encode_varint(stream, (uint64_t)value); +} + +static bool checkreturn pb_enc_uvarint(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + uint64_t value = 0; + + switch (field->data_size) + { + case 4: value = *(const uint32_t*)src; break; + case 8: value = *(const uint64_t*)src; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return pb_encode_varint(stream, value); +} + +static bool checkreturn pb_enc_svarint(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + int64_t value = 0; + + switch (field->data_size) + { + case 4: value = *(const int32_t*)src; break; + case 8: value = *(const int64_t*)src; break; + default: PB_RETURN_ERROR(stream, "invalid data_size"); + } + + return pb_encode_svarint(stream, value); +} + +static bool checkreturn pb_enc_fixed64(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + UNUSED(field); + return pb_encode_fixed64(stream, src); +} + +static bool checkreturn pb_enc_fixed32(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + UNUSED(field); + return pb_encode_fixed32(stream, src); +} + +static bool checkreturn pb_enc_bytes(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + const pb_bytes_array_t *bytes = (const pb_bytes_array_t*)src; + + if (src == NULL) + { + /* Threat null pointer as an empty bytes field */ + return pb_encode_string(stream, NULL, 0); + } + + if (PB_ATYPE(field->type) == PB_ATYPE_STATIC && + PB_BYTES_ARRAY_T_ALLOCSIZE(bytes->size) > field->data_size) + { + PB_RETURN_ERROR(stream, "bytes size exceeded"); + } + + return pb_encode_string(stream, bytes->bytes, bytes->size); +} + +static bool checkreturn pb_enc_string(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + /* strnlen() is not always available, so just use a loop */ + size_t size = 0; + size_t max_size = field->data_size; + const char *p = (const char*)src; + + if (PB_ATYPE(field->type) == PB_ATYPE_POINTER) + max_size = (size_t)-1; + + if (src == NULL) + { + size = 0; /* Threat null pointer as an empty string */ + } + else + { + while (size < max_size && *p != '\0') + { + size++; + p++; + } + } + + return pb_encode_string(stream, (const uint8_t*)src, size); +} + +static bool checkreturn pb_enc_submessage(pb_ostream_t *stream, const pb_field_t *field, const void *src) +{ + if (field->ptr == NULL) + PB_RETURN_ERROR(stream, "invalid field descriptor"); + + return pb_encode_submessage(stream, (const pb_field_t*)field->ptr, src); +} + diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.h b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.h new file mode 100644 index 0000000..00325e5 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_encode.h @@ -0,0 +1,154 @@ +/* pb_encode.h: Functions to encode protocol buffers. Depends on pb_encode.c. + * The main function is pb_encode. You also need an output stream, and the + * field descriptions created by nanopb_generator.py. + */ + +#ifndef _PB_ENCODE_H_ +#define _PB_ENCODE_H_ + +#include "pb.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* Structure for defining custom output streams. You will need to provide + * a callback function to write the bytes to your storage, which can be + * for example a file or a network socket. + * + * The callback must conform to these rules: + * + * 1) Return false on IO errors. This will cause encoding to abort. + * 2) You can use state to store your own data (e.g. buffer pointer). + * 3) pb_write will update bytes_written after your callback runs. + * 4) Substreams will modify max_size and bytes_written. Don't use them + * to calculate any pointers. + */ +struct _pb_ostream_t +{ +#ifdef PB_BUFFER_ONLY + /* Callback pointer is not used in buffer-only configuration. + * Having an int pointer here allows binary compatibility but + * gives an error if someone tries to assign callback function. + * Also, NULL pointer marks a 'sizing stream' that does not + * write anything. + */ + int *callback; +#else + bool (*callback)(pb_ostream_t *stream, const uint8_t *buf, size_t count); +#endif + void *state; /* Free field for use by callback implementation. */ + size_t max_size; /* Limit number of output bytes written (or use SIZE_MAX). */ + size_t bytes_written; /* Number of bytes written so far. */ + +#ifndef PB_NO_ERRMSG + const char *errmsg; +#endif +}; + +/*************************** + * Main encoding functions * + ***************************/ + +/* Encode a single protocol buffers message from C structure into a stream. + * Returns true on success, false on any failure. + * The actual struct pointed to by src_struct must match the description in fields. + * All required fields in the struct are assumed to have been filled in. + * + * Example usage: + * MyMessage msg = {}; + * uint8_t buffer[64]; + * pb_ostream_t stream; + * + * msg.field1 = 42; + * stream = pb_ostream_from_buffer(buffer, sizeof(buffer)); + * pb_encode(&stream, MyMessage_fields, &msg); + */ +bool pb_encode(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct); + +/* Same as pb_encode, but prepends the length of the message as a varint. + * Corresponds to writeDelimitedTo() in Google's protobuf API. + */ +bool pb_encode_delimited(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct); + +/* Encode the message to get the size of the encoded data, but do not store + * the data. */ +bool pb_get_encoded_size(size_t *size, const pb_field_t fields[], const void *src_struct); + +/************************************** + * Functions for manipulating streams * + **************************************/ + +/* Create an output stream for writing into a memory buffer. + * The number of bytes written can be found in stream.bytes_written after + * encoding the message. + * + * Alternatively, you can use a custom stream that writes directly to e.g. + * a file or a network socket. + */ +pb_ostream_t pb_ostream_from_buffer(uint8_t *buf, size_t bufsize); + +/* Pseudo-stream for measuring the size of a message without actually storing + * the encoded data. + * + * Example usage: + * MyMessage msg = {}; + * pb_ostream_t stream = PB_OSTREAM_SIZING; + * pb_encode(&stream, MyMessage_fields, &msg); + * printf("Message size is %d\n", stream.bytes_written); + */ +#ifndef PB_NO_ERRMSG +#define PB_OSTREAM_SIZING {0,0,0,0,0} +#else +#define PB_OSTREAM_SIZING {0,0,0,0} +#endif + +/* Function to write into a pb_ostream_t stream. You can use this if you need + * to append or prepend some custom headers to the message. + */ +bool pb_write(pb_ostream_t *stream, const uint8_t *buf, size_t count); + + +/************************************************ + * Helper functions for writing field callbacks * + ************************************************/ + +/* Encode field header based on type and field number defined in the field + * structure. Call this from the callback before writing out field contents. */ +bool pb_encode_tag_for_field(pb_ostream_t *stream, const pb_field_t *field); + +/* Encode field header by manually specifing wire type. You need to use this + * if you want to write out packed arrays from a callback field. */ +bool pb_encode_tag(pb_ostream_t *stream, pb_wire_type_t wiretype, uint32_t field_number); + +/* Encode an integer in the varint format. + * This works for bool, enum, int32, int64, uint32 and uint64 field types. */ +bool pb_encode_varint(pb_ostream_t *stream, uint64_t value); + +/* Encode an integer in the zig-zagged svarint format. + * This works for sint32 and sint64. */ +bool pb_encode_svarint(pb_ostream_t *stream, int64_t value); + +/* Encode a string or bytes type field. For strings, pass strlen(s) as size. */ +bool pb_encode_string(pb_ostream_t *stream, const uint8_t *buffer, size_t size); + +/* Encode a fixed32, sfixed32 or float value. + * You need to pass a pointer to a 4-byte wide C variable. */ +bool pb_encode_fixed32(pb_ostream_t *stream, const void *value); + +/* Encode a fixed64, sfixed64 or double value. + * You need to pass a pointer to a 8-byte wide C variable. */ +bool pb_encode_fixed64(pb_ostream_t *stream, const void *value); + +/* Encode a submessage field. + * You need to pass the pb_field_t array and pointer to struct, just like + * with pb_encode(). This internally encodes the submessage twice, first to + * calculate message size and then to actually write it out. + */ +bool pb_encode_submessage(pb_ostream_t *stream, const pb_field_t fields[], const void *src_struct); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr.h b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr.h new file mode 100644 index 0000000..319545c --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr.h @@ -0,0 +1,127 @@ +/* This is an example of a header file for platforms/compilers that do + * not come with stdint.h/stddef.h/stdbool.h/string.h. To use it, define + * PB_SYSTEM_HEADER as "pb_syshdr.h", including the quotes, and add the + * extra folder to your include path. + * + * It is very likely that you will need to customize this file to suit + * your platform. For any compiler that supports C99, this file should + * not be necessary. + * + * KUKA: Added VXWORKS support + */ + +#ifndef _PB_SYSHDR_H_ +#define _PB_SYSHDR_H_ + +/* KUKA VxWorks 6.8 support */ +#ifdef VXWORKS + #define HAVE_STRING_H + #define HAVE_STDLIB_H +#ifdef _WRS_KERNEL + #include // int32_t, int64_t, ... + #define HAVE_STDINT_H_ALTERNATIVE + #define HAVE_STDDEF_H_ALTERNATIVE +#else + #define HAVE_STDINT_H + #define HAVE_STDDEF_H +#endif // _WRS_KERNEL +#endif // VXWORKS + +/* KUKA: size_t is defined in stddef.h, stdlib.h or string.h */ +#if !defined(HAVE_STDDEF_H) && !defined(HAVE_STDLIB_H) && !defined(HAVE_STRING_H) +typedef uint32_t size_t; +#endif + + + +/* stdint.h subset */ +#ifdef HAVE_STDINT_H +#include +#else +#ifndef HAVE_STDINT_H_ALTERNATIVE +/* You will need to modify these to match the word size of your platform. */ +typedef signed char int8_t; +typedef unsigned char uint8_t; +typedef signed short int16_t; +typedef unsigned short uint16_t; +typedef signed int int32_t; +typedef unsigned int uint32_t; +typedef signed long long int64_t; +typedef unsigned long long uint64_t; +#endif // HAVE_STDINT_H_ALTERNATIVE +#endif // HAVE_STDINT_H + +/* stddef.h subset */ +#ifdef HAVE_STDDEF_H +#include +#else +#ifndef HAVE_STDDEF_H_ALTERNATIVE +#define offsetof(st, m) ((size_t)(&((st *)0)->m)) +#ifndef NULL +#define NULL 0 +#endif // NULL +#endif // HAVE_STDDEF_H_ALTERNATIVE +#endif // HAVE_STDDEF_H + +/* stdbool.h subset */ +#ifdef HAVE_STDBOOL_H +#include +#else + +#ifndef __cplusplus +typedef int bool; +#define false 0 +#define true 1 +#endif + +#endif // HAVE_STDBOOL_H + +/* stdlib.h subset */ +#ifdef PB_ENABLE_MALLOC +#ifdef HAVE_STDLIB_H +#include +#else +void *realloc(void *ptr, size_t size); +void free(void *ptr); +#endif // HAVE_STDLIB_H +#endif // PB_ENABLE_MALLOC + +/* string.h subset */ +#ifdef HAVE_STRING_H +#include +#else + +/* Implementations are from the Public Domain C Library (PDCLib). */ +static size_t strlen( const char * s ) +{ + size_t rc = 0; + while ( s[rc] ) + { + ++rc; + } + return rc; +} + +static void * memcpy( void *s1, const void *s2, size_t n ) +{ + char * dest = (char *) s1; + const char * src = (const char *) s2; + while ( n-- ) + { + *dest++ = *src++; + } + return s1; +} + +static void * memset( void * s, int c, size_t n ) +{ + unsigned char * p = (unsigned char *) s; + while ( n-- ) + { + *p++ = (unsigned char) c; + } + return s; +} +#endif // HAVE_STRING_H + +#endif // _PB_SYSHDR_H_ diff --git a/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr_win.h b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr_win.h new file mode 100644 index 0000000..e61d5b2 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/nanopb-0.2.8/pb_syshdr_win.h @@ -0,0 +1,119 @@ +/* This is a headerfile customized for Microsoft Visual Studio 2010 + * - based on example of a header file for platforms/compilers that do + * not come with stdint.h/stddef.h/stdbool.h/string.h. To use it, define + * PB_SYSTEM_HEADER as "pb_syshdr_win.h", including the quotes, and add the + * extra folder to your include path. + * + * Authorship: This file was altered/created by KUKA Deutschland GmbH, Augsburg, Germany in 2014 + * + */ + +#ifndef _PB_SYSHDR_WIN_H_ +#define _PB_SYSHDR_WIN_H_ + +/* stdint.h subset */ +#ifdef HAVE_STDINT_H +#include +#else +/* You will need to modify these to match the word size of your platform. */ +typedef signed char int8_t; +typedef unsigned char uint8_t; +typedef signed short int16_t; +typedef unsigned short uint16_t; +typedef signed int int32_t; +typedef unsigned int uint32_t; +typedef signed long long int64_t; +typedef unsigned long long uint64_t; + +// from stdint.h +#define INT8_MAX 0x7f +#define INT16_MAX 0x7fff +#define INT32_MAX 0x7fffffff +#define UINT8_MAX 0xffU +#define UINT16_MAX 0xffffU +#define UINT32_MAX 0xffffffffU +#endif + +/* stddef.h subset */ +#ifdef HAVE_STDDEF_H +#include +#else + +//typedef uint32_t size_t; // wird dieser typedef wirklich bentigt?!? +#ifndef offsetof +#define offsetof(st, m) ((size_t)(&((st *)0)->m)) +#endif + +#ifndef NULL +#define NULL 0 +#endif + +#endif + +/* stdbool.h subset */ +#ifdef HAVE_STDBOOL_H +#include +#else + +#ifndef __cplusplus +typedef int bool; +#define false 0 +#define true 1 +#endif + +#endif + +/* stdlib.h subset */ +#ifdef PB_ENABLE_MALLOC +#ifdef HAVE_STDLIB_H +#include +#else +void *realloc(void *ptr, size_t size); +void free(void *ptr); +#endif +#endif + +/* string.h subset */ +#ifdef HAVE_STRING_H +#include +#else + +#ifndef WIN32 +/* Implementations are from the Public Domain C Library (PDCLib). */ +static size_t strlen( const char * s ) +{ + size_t rc = 0; + while ( s[rc] ) + { + ++rc; + } + return rc; +} + +static void * memcpy( void *s1, const void *s2, size_t n ) +{ + char * dest = (char *) s1; + const char * src = (const char *) s2; + while ( n-- ) + { + *dest++ = *src++; + } + return s1; +} + +static void * memset( void * s, int c, size_t n ) +{ + unsigned char * p = (unsigned char *) s; + while ( n-- ) + { + *p++ = (unsigned char) c; + } + return s; +} +#else +#include +#endif + +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/Makefile b/src/iiwa_controller/external/libFRI/src/protobuf/Makefile new file mode 100644 index 0000000..99e8c24 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/Makefile @@ -0,0 +1,19 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CC_SRC = pb_frimessages_callbacks.c + +CXX_SRC = friMonitoringMessageDecoder.cpp \ + friCommandMessageEncoder.cpp + +INC_DIR += $(NANOPB_DIR) $(PROTOBUF_GEN_DIR) +CFLAGS += +CXXFLAGS += +LDFLAGS += + +################################################################################ +### Include general makefile (at the end) +################################################################################ + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.cpp b/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.cpp new file mode 100644 index 0000000..9f7f7a1 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.cpp @@ -0,0 +1,121 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include +#include "friCommandMessageEncoder.h" +#include "pb_encode.h" + +using namespace KUKA::FRI; + +//****************************************************************************** +CommandMessageEncoder::CommandMessageEncoder(FRICommandMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +CommandMessageEncoder::~CommandMessageEncoder() +{ + +} + +//****************************************************************************** +void CommandMessageEncoder::initMessage() +{ + m_pMessage->has_commandData = false; + m_pMessage->has_endOfMessageData = false; + m_pMessage->commandData.has_jointPosition = false; + m_pMessage->commandData.has_cartesianWrenchFeedForward = false; + m_pMessage->commandData.has_jointTorque = false; + m_pMessage->commandData.commandedTransformations_count = 0; + m_pMessage->header.messageIdentifier = 0; + // init with 0. Necessary for creating the correct reflected sequence count in the monitoring msg + m_pMessage->header.sequenceCounter = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + + m_pMessage->commandData.writeIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointPosition.value, + &m_tRecvContainer.jointPosition); + map_repeatedDouble(FRI_MANAGER_NANOPB_ENCODE, m_nNum, + &m_pMessage->commandData.jointTorque.value, + &m_tRecvContainer.jointTorque); + + // nanopb encoding needs to know how many elements the static array contains + // a Cartesian wrench feed forward vector always contains 6 elements + m_pMessage->commandData.cartesianWrenchFeedForward.element_count = 6; +} + +//****************************************************************************** +bool CommandMessageEncoder::encode(char* buffer, int& size) +{ + // generate stream for encoding + pb_ostream_t stream = pb_ostream_from_buffer((uint8_t*)buffer, FRI_COMMAND_MSG_MAX_SIZE); + // encode monitoring Message to stream + bool status = pb_encode(&stream, FRICommandMessage_fields, m_pMessage); + size = stream.bytes_written; + if (!status) + { + printf("!!encoding error: %s!!\n", PB_GET_ERROR(&stream)); + } + return status; +} diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.h b/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.h new file mode 100644 index 0000000..542c1bf --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/friCommandMessageEncoder.h @@ -0,0 +1,118 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_COMMANDMESSAGEENCODER_H +#define _KUKA_FRI_COMMANDMESSAGEENCODER_H + + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_COMMAND_MSG_MAX_SIZE = 1500; //!< max size of a FRI command message + + class CommandMessageEncoder + { + + public: + + CommandMessageEncoder(FRICommandMessage* pMessage, int num); + + ~CommandMessageEncoder(); + + bool encode(char* buffer, int& size); + + private: + + struct LocalCommandDataContainer + { + tRepeatedDoubleArguments jointPosition; + tRepeatedDoubleArguments jointTorque; + + LocalCommandDataContainer() + { + init_repeatedDouble(&jointPosition); + init_repeatedDouble(&jointTorque); + } + + ~LocalCommandDataContainer() + { + free_repeatedDouble(&jointPosition); + free_repeatedDouble(&jointTorque); + } + }; + + int m_nNum; + + LocalCommandDataContainer m_tRecvContainer; + FRICommandMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_COMMANDMESSAGEENCODER_H diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp b/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp new file mode 100644 index 0000000..83d1e22 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.cpp @@ -0,0 +1,145 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#include +#include "friMonitoringMessageDecoder.h" +#include "pb_decode.h" + + +using namespace KUKA::FRI; + +//****************************************************************************** +MonitoringMessageDecoder::MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num) + : m_nNum(num), m_pMessage(pMessage) +{ + initMessage(); +} + +//****************************************************************************** +MonitoringMessageDecoder::~MonitoringMessageDecoder() +{ + +} + +//****************************************************************************** +void MonitoringMessageDecoder::initMessage() +{ + // set initial data + // it is assumed that no robot information and monitoring data is available and therefore the + // optional fields are initialized with false + m_pMessage->has_robotInfo = false; + m_pMessage->has_monitorData = false; + m_pMessage->has_connectionInfo = true; + m_pMessage->has_ipoData = false; + m_pMessage->requestedTransformations_count = 0; + m_pMessage->has_endOfMessageData = false; + + + m_pMessage->header.messageIdentifier = 0; + m_pMessage->header.reflectedSequenceCounter = 0; + m_pMessage->header.sequenceCounter = 0; + + m_pMessage->connectionInfo.sessionState = FRISessionState_IDLE; + m_pMessage->connectionInfo.quality = FRIConnectionQuality_POOR; + + m_pMessage->monitorData.readIORequest_count = 0; + + // allocate and map memory for protobuf repeated structures + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredJointPosition.value, + &m_tSendContainer.m_AxQMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.measuredTorque.value, + &m_tSendContainer.m_AxTauMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedJointPosition.value, + &m_tSendContainer.m_AxQCmdT1mLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.commandedTorque.value, + &m_tSendContainer.m_AxTauCmdLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->monitorData.externalTorque.value, + &m_tSendContainer.m_AxTauExtMsrLocal); + + map_repeatedDouble(FRI_MANAGER_NANOPB_DECODE,m_nNum, + &m_pMessage->ipoData.jointPosition.value, + &m_tSendContainer.m_AxQCmdIPO); + + map_repeatedInt(FRI_MANAGER_NANOPB_DECODE, m_nNum, + &m_pMessage->robotInfo.driveState, + &m_tSendContainer.m_AxDriveStateLocal); +} + +//****************************************************************************** +bool MonitoringMessageDecoder::decode(char* buffer, int size) +{ + pb_istream_t stream = pb_istream_from_buffer((uint8_t*)buffer, size); + + bool status = pb_decode(&stream, FRIMonitoringMessage_fields, m_pMessage); + if (!status) + { + printf("!!decoding error: %s!!\n", PB_GET_ERROR(&stream)); + } + + return status; +} diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.h b/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.h new file mode 100644 index 0000000..2fc2285 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/friMonitoringMessageDecoder.h @@ -0,0 +1,133 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _KUKA_FRI_MONITORINGMESSAGEDECODER_H +#define _KUKA_FRI_MONITORINGMESSAGEDECODER_H + +#include "FRIMessages.pb.h" +#include "pb_frimessages_callbacks.h" + + +namespace KUKA +{ +namespace FRI +{ + + static const int FRI_MONITOR_MSG_MAX_SIZE = 1500; //!< max size of a FRI monitoring message + + + class MonitoringMessageDecoder + { + + public: + + MonitoringMessageDecoder(FRIMonitoringMessage* pMessage, int num); + + ~MonitoringMessageDecoder(); + + bool decode(char* buffer, int size); + + + private: + + struct LocalMonitoringDataContainer + { + tRepeatedDoubleArguments m_AxQMsrLocal; + tRepeatedDoubleArguments m_AxTauMsrLocal; + tRepeatedDoubleArguments m_AxQCmdT1mLocal; + tRepeatedDoubleArguments m_AxTauCmdLocal; + tRepeatedDoubleArguments m_AxTauExtMsrLocal; + tRepeatedIntArguments m_AxDriveStateLocal; + tRepeatedDoubleArguments m_AxQCmdIPO; + + LocalMonitoringDataContainer() + { + init_repeatedDouble(&m_AxQMsrLocal); + init_repeatedDouble(&m_AxTauMsrLocal); + init_repeatedDouble(&m_AxQCmdT1mLocal); + init_repeatedDouble(&m_AxTauCmdLocal); + init_repeatedDouble(&m_AxTauExtMsrLocal); + init_repeatedDouble(&m_AxQCmdIPO); + init_repeatedInt(&m_AxDriveStateLocal); + } + + ~LocalMonitoringDataContainer() + { + free_repeatedDouble(&m_AxQMsrLocal); + free_repeatedDouble(&m_AxTauMsrLocal); + free_repeatedDouble(&m_AxQCmdT1mLocal); + free_repeatedDouble(&m_AxTauCmdLocal); + free_repeatedDouble(&m_AxTauExtMsrLocal); + free_repeatedDouble(&m_AxQCmdIPO); + free_repeatedInt(&m_AxDriveStateLocal); + } + }; + + int m_nNum; + + LocalMonitoringDataContainer m_tSendContainer; + FRIMonitoringMessage* m_pMessage; + + void initMessage(); + }; + +} +} + +#endif // _KUKA_FRI_MONITORINGMESSAGEDECODER_H diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.c b/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.c new file mode 100644 index 0000000..e76620f --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.c @@ -0,0 +1,267 @@ +/** + + The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + + + \file + \version {1.16} + */ +#include +#include + +#include "pb_frimessages_callbacks.h" +#include "pb_encode.h" +#include "pb_decode.h" + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + size_t i = 0; + + tRepeatedDoubleArguments* arguments = 0; + size_t count = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = ((tRepeatedDoubleArguments*) (*arg)); + + count = arguments->max_size; + values = arguments->value; + + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + + if (!pb_encode_fixed64(stream, &values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedDoubleArguments* arguments = 0; + size_t i = 0; + double* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + + arguments = (tRepeatedDoubleArguments*) *arg; + i = arguments->size; + values = arguments->value; + + if (values == NULL) + { + return true; + } + + if (!pb_decode_fixed64(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, void * const *arg) +{ + int i = 0; + tRepeatedIntArguments* arguments = 0; + int count = 0; + int64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + count = arguments->max_size; + values = arguments->value; + for (i = 0; i < count; i++) + { + + if (!pb_encode_tag_for_field(stream, field)) + { + return false; + } + if (!pb_encode_varint(stream, values[i])) + { + return false; + } + } + return true; +} + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, void **arg) +{ + tRepeatedIntArguments* arguments = 0; + size_t i = 0; + uint64_t* values = 0; + + if (arg == NULL || *arg == NULL) + { + return false; + } + arguments = (tRepeatedIntArguments*) *arg; + + i = arguments->size; + values = (uint64_t*) arguments->value; + if (values == NULL) + { + return true; + } + + if (!pb_decode_varint(stream, &values[i])) + { + return false; + } + + arguments->size++; + if (arguments->size >= arguments->max_size) + { + arguments->size = 0; + } + return true; +} + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedDoubleArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + values->funcs.encode = &encode_repeatedDouble; + } + else + { + values->funcs.decode = &decode_repeatedDouble; + } + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (double*) malloc(numDOF * sizeof(double)); + + } + values->arg = arg; +} + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, pb_callback_t *values, tRepeatedIntArguments *arg) +{ + // IMPORTANT: the callbacks are stored in a union, therefor a message object + // must be exclusive defined for transmission or reception + if (dir == FRI_MANAGER_NANOPB_ENCODE) + { + // set the encode callback function + values->funcs.encode = &encode_repeatedInt; + } + else + { + // set the decode callback function + values->funcs.decode = &decode_repeatedInt; + } + // map the robot drive state from the container to message field + arg->max_size = numDOF; + arg->size = 0; + if (numDOF > 0) + { + arg->value = (int64_t*) malloc(numDOF * sizeof(int64_t)); + + } + values->arg = arg; +} + +void init_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void init_repeatedInt(tRepeatedIntArguments *arg) +{ + arg->size = 0; + arg->max_size = 0; + arg->value = NULL; +} + +void free_repeatedDouble(tRepeatedDoubleArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} + +void free_repeatedInt(tRepeatedIntArguments *arg) +{ + if (arg->value != NULL) + free(arg->value); +} diff --git a/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.h b/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.h new file mode 100644 index 0000000..e77bc2c --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf/pb_frimessages_callbacks.h @@ -0,0 +1,121 @@ +/** + +The following license terms and conditions apply, unless a redistribution +agreement or other license is obtained by KUKA Deutschland GmbH, Augsburg, Germany. + +SCOPE + +The software KUKA Sunrise.Connectivity FRI Client SDK is targeted to work in +conjunction with the KUKA Sunrise.Connectivity FastRobotInterface toolkit. +In the following, the term software refers to all material directly +belonging to the provided SDK Software development kit, particularly source +code, libraries, binaries, manuals and technical documentation. + +COPYRIGHT + +All Rights Reserved +Copyright (C) 2014-2018 +KUKA Deutschland GmbH +Augsburg, Germany + +LICENSE + +Redistribution and use of the software in source and binary forms, with or +without modification, are permitted provided that the following conditions are +met: +a) The software is used in conjunction with KUKA products only. +b) Redistributions of source code must retain the above copyright notice, this +list of conditions and the disclaimer. +c) Redistributions in binary form must reproduce the above copyright notice, +this list of conditions and the disclaimer in the documentation and/or other +materials provided with the distribution. Altered source code of the +redistribution must be made available upon request with the distribution. +d) Modification and contributions to the original software provided by KUKA +must be clearly marked and the authorship must be stated. +e) Neither the name of KUKA nor the trademarks owned by KUKA may be used to +endorse or promote products derived from this software without specific prior +written permission. + +DISCLAIMER OF WARRANTY + +The Software is provided "AS IS" and "WITH ALL FAULTS," without warranty of +any kind, including without limitation the warranties of merchantability, +fitness for a particular purpose and non-infringement. +KUKA makes no warranty that the Software is free of defects or is suitable for +any particular purpose. In no event shall KUKA be responsible for loss or +damages arising from the installation or use of the Software, including but +not limited to any indirect, punitive, special, incidental or consequential +damages of any character including, without limitation, damages for loss of +goodwill, work stoppage, computer failure or malfunction, or any and all other +commercial damages or losses. +The entire risk to the quality and performance of the Software is not borne by +KUKA. Should the Software prove defective, KUKA is not liable for the entire +cost of any service and repair. + + + +\file +\version {1.16} +*/ +#ifndef _pb_frimessages_callbacks_H +#define _pb_frimessages_callbacks_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include "pb.h" +#include "FRIMessages.pb.h" + +/** container for repeated double elements */ +typedef struct repeatedDoubleArguments { + size_t size; + size_t max_size; + double* value; +} tRepeatedDoubleArguments; + +/** container for repeated integer elements */ +typedef struct repeatedIntArguments { + size_t size; + size_t max_size; + int64_t* value; +} tRepeatedIntArguments; + +/** enumeration for direction (encoding/decoding) */ +typedef enum DIRECTION { + FRI_MANAGER_NANOPB_DECODE = 0, //!< Argument um eine + FRI_MANAGER_NANOPB_ENCODE = 1 //!< +} eNanopbCallbackDirection; + + +bool encode_repeatedDouble(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedDouble(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +bool encode_repeatedInt(pb_ostream_t *stream, const pb_field_t *field, + void * const *arg); + +bool decode_repeatedInt(pb_istream_t *stream, const pb_field_t *field, + void **arg); + +void map_repeatedDouble(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedDoubleArguments *arg); + +void map_repeatedInt(eNanopbCallbackDirection dir, int numDOF, + pb_callback_t *values, tRepeatedIntArguments *arg); + +void init_repeatedDouble(tRepeatedDoubleArguments *arg); + +void init_repeatedInt(tRepeatedIntArguments *arg); + +void free_repeatedDouble(tRepeatedDoubleArguments *arg); + +void free_repeatedInt(tRepeatedIntArguments *arg); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.c b/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.c new file mode 100644 index 0000000..f8dc2a8 --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.c @@ -0,0 +1,151 @@ +/* Automatically generated nanopb constant definitions */ +/* Generated by nanopb-0.2.8 at Tue Feb 14 12:42:03 2017. */ + +#include "FRIMessages.pb.h" + + + +const pb_field_t JointValues_fields[2] = { + PB_FIELD2( 1, DOUBLE , REPEATED, CALLBACK, FIRST, JointValues, value, value, 0), + PB_LAST_FIELD +}; + +const pb_field_t TimeStamp_fields[3] = { + PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, TimeStamp, sec, sec, 0), + PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, TimeStamp, nanosec, sec, 0), + PB_LAST_FIELD +}; + +const pb_field_t CartesianVector_fields[2] = { + PB_FIELD2( 1, DOUBLE , REPEATED, STATIC , FIRST, CartesianVector, element, element, 0), + PB_LAST_FIELD +}; + +const pb_field_t Checksum_fields[2] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, Checksum, crc32, crc32, 0), + PB_LAST_FIELD +}; + +const pb_field_t Transformation_fields[4] = { + PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, Transformation, name, name, 0), + PB_FIELD2( 2, DOUBLE , REPEATED, STATIC , OTHER, Transformation, matrix, name, 0), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, Transformation, timestamp, matrix, &TimeStamp_fields), + PB_LAST_FIELD +}; + +const pb_field_t FriIOValue_fields[6] = { + PB_FIELD2( 1, STRING , REQUIRED, STATIC , FIRST, FriIOValue, name, name, 0), + PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, type, name, 0), + PB_FIELD2( 3, ENUM , REQUIRED, STATIC , OTHER, FriIOValue, direction, type, 0), + PB_FIELD2( 4, UINT64 , OPTIONAL, STATIC , OTHER, FriIOValue, digitalValue, direction, 0), + PB_FIELD2( 5, DOUBLE , OPTIONAL, STATIC , OTHER, FriIOValue, analogValue, digitalValue, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageHeader_fields[4] = { + PB_FIELD2( 1, UINT32 , REQUIRED, STATIC , FIRST, MessageHeader, messageIdentifier, messageIdentifier, 0), + PB_FIELD2( 2, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, sequenceCounter, messageIdentifier, 0), + PB_FIELD2( 3, UINT32 , REQUIRED, STATIC , OTHER, MessageHeader, reflectedSequenceCounter, sequenceCounter, 0), + PB_LAST_FIELD +}; + +const pb_field_t ConnectionInfo_fields[5] = { + PB_FIELD2( 1, ENUM , REQUIRED, STATIC , FIRST, ConnectionInfo, sessionState, sessionState, 0), + PB_FIELD2( 2, ENUM , REQUIRED, STATIC , OTHER, ConnectionInfo, quality, sessionState, 0), + PB_FIELD2( 3, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, sendPeriod, quality, 0), + PB_FIELD2( 4, UINT32 , OPTIONAL, STATIC , OTHER, ConnectionInfo, receiveMultiplier, sendPeriod, 0), + PB_LAST_FIELD +}; + +const pb_field_t RobotInfo_fields[6] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, RobotInfo, numberOfJoints, numberOfJoints, 0), + PB_FIELD2( 2, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, safetyState, numberOfJoints, 0), + PB_FIELD2( 5, ENUM , REPEATED, CALLBACK, OTHER, RobotInfo, driveState, safetyState, 0), + PB_FIELD2( 6, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, operationMode, driveState, 0), + PB_FIELD2( 7, ENUM , OPTIONAL, STATIC , OTHER, RobotInfo, controlMode, operationMode, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageMonitorData_fields[8] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageMonitorData, measuredJointPosition, measuredJointPosition, &JointValues_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, measuredTorque, measuredJointPosition, &JointValues_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, commandedJointPosition, measuredTorque, &JointValues_fields), + PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, commandedTorque, commandedJointPosition, &JointValues_fields), + PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, externalTorque, commandedTorque, &JointValues_fields), + PB_FIELD2( 8, MESSAGE , REPEATED, STATIC , OTHER, MessageMonitorData, readIORequest, externalTorque, &FriIOValue_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, MessageMonitorData, timestamp, readIORequest, &TimeStamp_fields), + PB_LAST_FIELD +}; + +const pb_field_t MessageIpoData_fields[5] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageIpoData, jointPosition, jointPosition, &JointValues_fields), + PB_FIELD2( 10, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, clientCommandMode, jointPosition, 0), + PB_FIELD2( 11, ENUM , OPTIONAL, STATIC , OTHER, MessageIpoData, overlayType, clientCommandMode, 0), + PB_FIELD2( 12, DOUBLE , OPTIONAL, STATIC , OTHER, MessageIpoData, trackingPerformance, overlayType, 0), + PB_LAST_FIELD +}; + +const pb_field_t MessageCommandData_fields[6] = { + PB_FIELD2( 1, MESSAGE , OPTIONAL, STATIC , FIRST, MessageCommandData, jointPosition, jointPosition, &JointValues_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, cartesianWrenchFeedForward, jointPosition, &CartesianVector_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, MessageCommandData, jointTorque, cartesianWrenchFeedForward, &JointValues_fields), + PB_FIELD2( 4, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, commandedTransformations, jointTorque, &Transformation_fields), + PB_FIELD2( 5, MESSAGE , REPEATED, STATIC , OTHER, MessageCommandData, writeIORequest, commandedTransformations, &FriIOValue_fields), + PB_LAST_FIELD +}; + +const pb_field_t MessageEndOf_fields[3] = { + PB_FIELD2( 1, INT32 , OPTIONAL, STATIC , FIRST, MessageEndOf, messageLength, messageLength, 0), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, MessageEndOf, messageChecksum, messageLength, &Checksum_fields), + PB_LAST_FIELD +}; + +const pb_field_t FRIMonitoringMessage_fields[8] = { + PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRIMonitoringMessage, header, header, &MessageHeader_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, robotInfo, header, &RobotInfo_fields), + PB_FIELD2( 3, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, monitorData, robotInfo, &MessageMonitorData_fields), + PB_FIELD2( 4, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, connectionInfo, monitorData, &ConnectionInfo_fields), + PB_FIELD2( 5, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, ipoData, connectionInfo, &MessageIpoData_fields), + PB_FIELD2( 6, MESSAGE , REPEATED, STATIC , OTHER, FRIMonitoringMessage, requestedTransformations, ipoData, &Transformation_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRIMonitoringMessage, endOfMessageData, requestedTransformations, &MessageEndOf_fields), + PB_LAST_FIELD +}; + +const pb_field_t FRICommandMessage_fields[4] = { + PB_FIELD2( 1, MESSAGE , REQUIRED, STATIC , FIRST, FRICommandMessage, header, header, &MessageHeader_fields), + PB_FIELD2( 2, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, commandData, header, &MessageCommandData_fields), + PB_FIELD2( 15, MESSAGE , OPTIONAL, STATIC , OTHER, FRICommandMessage, endOfMessageData, commandData, &MessageEndOf_fields), + PB_LAST_FIELD +}; + + +/* Check that field information fits in pb_field_t */ +#if !defined(PB_FIELD_32BIT) +/* If you get an error here, it means that you need to define PB_FIELD_32BIT + * compile-time option. You can do that in pb.h or on compiler command line. + * + * The reason you need to do this is that some of your messages contain tag + * numbers or field sizes that are larger than what can fit in 8 or 16 bit + * field descriptors. + */ +STATIC_ASSERT((pb_membersize(Transformation, timestamp) < 65536 && pb_membersize(MessageMonitorData, measuredJointPosition) < 65536 && pb_membersize(MessageMonitorData, measuredTorque) < 65536 && pb_membersize(MessageMonitorData, commandedJointPosition) < 65536 && pb_membersize(MessageMonitorData, commandedTorque) < 65536 && pb_membersize(MessageMonitorData, externalTorque) < 65536 && pb_membersize(MessageMonitorData, readIORequest[0]) < 65536 && pb_membersize(MessageMonitorData, timestamp) < 65536 && pb_membersize(MessageIpoData, jointPosition) < 65536 && pb_membersize(MessageCommandData, jointPosition) < 65536 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 65536 && pb_membersize(MessageCommandData, jointTorque) < 65536 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 65536 && pb_membersize(MessageCommandData, writeIORequest[0]) < 65536 && pb_membersize(MessageEndOf, messageChecksum) < 65536 && pb_membersize(FRIMonitoringMessage, header) < 65536 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 65536 && pb_membersize(FRIMonitoringMessage, robotInfo) < 65536 && pb_membersize(FRIMonitoringMessage, monitorData) < 65536 && pb_membersize(FRIMonitoringMessage, ipoData) < 65536 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 65536 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 65536 && pb_membersize(FRICommandMessage, header) < 65536 && pb_membersize(FRICommandMessage, commandData) < 65536 && pb_membersize(FRICommandMessage, endOfMessageData) < 65536), YOU_MUST_DEFINE_PB_FIELD_32BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_Transformation_FriIOValue_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) +#endif + +#if !defined(PB_FIELD_16BIT) && !defined(PB_FIELD_32BIT) +/* If you get an error here, it means that you need to define PB_FIELD_16BIT + * compile-time option. You can do that in pb.h or on compiler command line. + * + * The reason you need to do this is that some of your messages contain tag + * numbers or field sizes that are larger than what can fit in the default + * 8 bit descriptors. + */ +STATIC_ASSERT((pb_membersize(Transformation, timestamp) < 256 && pb_membersize(MessageMonitorData, measuredJointPosition) < 256 && pb_membersize(MessageMonitorData, measuredTorque) < 256 && pb_membersize(MessageMonitorData, commandedJointPosition) < 256 && pb_membersize(MessageMonitorData, commandedTorque) < 256 && pb_membersize(MessageMonitorData, externalTorque) < 256 && pb_membersize(MessageMonitorData, readIORequest[0]) < 256 && pb_membersize(MessageMonitorData, timestamp) < 256 && pb_membersize(MessageIpoData, jointPosition) < 256 && pb_membersize(MessageCommandData, jointPosition) < 256 && pb_membersize(MessageCommandData, cartesianWrenchFeedForward) < 256 && pb_membersize(MessageCommandData, jointTorque) < 256 && pb_membersize(MessageCommandData, commandedTransformations[0]) < 256 && pb_membersize(MessageCommandData, writeIORequest[0]) < 256 && pb_membersize(MessageEndOf, messageChecksum) < 256 && pb_membersize(FRIMonitoringMessage, header) < 256 && pb_membersize(FRIMonitoringMessage, connectionInfo) < 256 && pb_membersize(FRIMonitoringMessage, robotInfo) < 256 && pb_membersize(FRIMonitoringMessage, monitorData) < 256 && pb_membersize(FRIMonitoringMessage, ipoData) < 256 && pb_membersize(FRIMonitoringMessage, requestedTransformations[0]) < 256 && pb_membersize(FRIMonitoringMessage, endOfMessageData) < 256 && pb_membersize(FRICommandMessage, header) < 256 && pb_membersize(FRICommandMessage, commandData) < 256 && pb_membersize(FRICommandMessage, endOfMessageData) < 256), YOU_MUST_DEFINE_PB_FIELD_16BIT_FOR_MESSAGES_JointValues_TimeStamp_CartesianVector_Checksum_Transformation_FriIOValue_MessageHeader_ConnectionInfo_RobotInfo_MessageMonitorData_MessageIpoData_MessageCommandData_MessageEndOf_FRIMonitoringMessage_FRICommandMessage) +#endif + + +/* On some platforms (such as AVR), double is really float. + * These are not directly supported by nanopb, but see example_avr_double. + * To get rid of this error, remove any double fields from your .proto. + */ +STATIC_ASSERT(sizeof(double) == 8, DOUBLE_MUST_BE_8_BYTES) + diff --git a/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.h b/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.h new file mode 100644 index 0000000..fbc031b --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf_gen/FRIMessages.pb.h @@ -0,0 +1,303 @@ +/* Automatically generated nanopb header */ +/* Generated by nanopb-0.2.8 at Tue Feb 14 12:42:03 2017. */ + +#ifndef _PB_FRIMESSAGES_PB_H_ +#define _PB_FRIMESSAGES_PB_H_ +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Enum definitions */ +typedef enum _FRISessionState { + FRISessionState_IDLE = 0, + FRISessionState_MONITORING_WAIT = 1, + FRISessionState_MONITORING_READY = 2, + FRISessionState_COMMANDING_WAIT = 3, + FRISessionState_COMMANDING_ACTIVE = 4 +} FRISessionState; + +typedef enum _FRIConnectionQuality { + FRIConnectionQuality_POOR = 0, + FRIConnectionQuality_FAIR = 1, + FRIConnectionQuality_GOOD = 2, + FRIConnectionQuality_EXCELLENT = 3 +} FRIConnectionQuality; + +typedef enum _SafetyState { + SafetyState_NORMAL_OPERATION = 0, + SafetyState_SAFETY_STOP_LEVEL_0 = 1, + SafetyState_SAFETY_STOP_LEVEL_1 = 2, + SafetyState_SAFETY_STOP_LEVEL_2 = 3 +} SafetyState; + +typedef enum _OperationMode { + OperationMode_TEST_MODE_1 = 0, + OperationMode_TEST_MODE_2 = 1, + OperationMode_AUTOMATIC_MODE = 2 +} OperationMode; + +typedef enum _DriveState { + DriveState_OFF = 0, + DriveState_TRANSITIONING = 1, + DriveState_ACTIVE = 2 +} DriveState; + +typedef enum _ControlMode { + ControlMode_POSITION_CONTROLMODE = 0, + ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE = 1, + ControlMode_JOINT_IMPEDANCE_CONTROLMODE = 2, + ControlMode_NO_CONTROLMODE = 3 +} ControlMode; + +typedef enum _ClientCommandMode { + ClientCommandMode_NO_COMMAND_MODE = 0, + ClientCommandMode_POSITION = 1, + ClientCommandMode_WRENCH = 2, + ClientCommandMode_TORQUE = 3 +} ClientCommandMode; + +typedef enum _OverlayType { + OverlayType_NO_OVERLAY = 0, + OverlayType_JOINT = 1, + OverlayType_CARTESIAN = 2 +} OverlayType; + +typedef enum _FriIOType { + FriIOType_BOOLEAN = 0, + FriIOType_DIGITAL = 1, + FriIOType_ANALOG = 2 +} FriIOType; + +typedef enum _FriIODirection { + FriIODirection_INPUT = 0, + FriIODirection_OUTPUT = 1 +} FriIODirection; + +/* Struct definitions */ +typedef struct _CartesianVector { + size_t element_count; + double element[6]; +} CartesianVector; + +typedef struct _Checksum { + bool has_crc32; + int32_t crc32; +} Checksum; + +typedef struct _ConnectionInfo { + FRISessionState sessionState; + FRIConnectionQuality quality; + bool has_sendPeriod; + uint32_t sendPeriod; + bool has_receiveMultiplier; + uint32_t receiveMultiplier; +} ConnectionInfo; + +typedef struct _FriIOValue { + char name[64]; + FriIOType type; + FriIODirection direction; + bool has_digitalValue; + uint64_t digitalValue; + bool has_analogValue; + double analogValue; +} FriIOValue; + +typedef struct _JointValues { + pb_callback_t value; +} JointValues; + +typedef struct _MessageHeader { + uint32_t messageIdentifier; + uint32_t sequenceCounter; + uint32_t reflectedSequenceCounter; +} MessageHeader; + +typedef struct _RobotInfo { + bool has_numberOfJoints; + int32_t numberOfJoints; + bool has_safetyState; + SafetyState safetyState; + pb_callback_t driveState; + bool has_operationMode; + OperationMode operationMode; + bool has_controlMode; + ControlMode controlMode; +} RobotInfo; + +typedef struct _TimeStamp { + uint32_t sec; + uint32_t nanosec; +} TimeStamp; + +typedef struct _MessageEndOf { + bool has_messageLength; + int32_t messageLength; + bool has_messageChecksum; + Checksum messageChecksum; +} MessageEndOf; + +typedef struct _MessageIpoData { + bool has_jointPosition; + JointValues jointPosition; + bool has_clientCommandMode; + ClientCommandMode clientCommandMode; + bool has_overlayType; + OverlayType overlayType; + bool has_trackingPerformance; + double trackingPerformance; +} MessageIpoData; + +typedef struct _MessageMonitorData { + bool has_measuredJointPosition; + JointValues measuredJointPosition; + bool has_measuredTorque; + JointValues measuredTorque; + bool has_commandedJointPosition; + JointValues commandedJointPosition; + bool has_commandedTorque; + JointValues commandedTorque; + bool has_externalTorque; + JointValues externalTorque; + size_t readIORequest_count; + FriIOValue readIORequest[10]; + bool has_timestamp; + TimeStamp timestamp; +} MessageMonitorData; + +typedef struct _Transformation { + char name[64]; + size_t matrix_count; + double matrix[12]; + bool has_timestamp; + TimeStamp timestamp; +} Transformation; + +typedef struct _FRIMonitoringMessage { + MessageHeader header; + bool has_robotInfo; + RobotInfo robotInfo; + bool has_monitorData; + MessageMonitorData monitorData; + bool has_connectionInfo; + ConnectionInfo connectionInfo; + bool has_ipoData; + MessageIpoData ipoData; + size_t requestedTransformations_count; + Transformation requestedTransformations[5]; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRIMonitoringMessage; + +typedef struct _MessageCommandData { + bool has_jointPosition; + JointValues jointPosition; + bool has_cartesianWrenchFeedForward; + CartesianVector cartesianWrenchFeedForward; + bool has_jointTorque; + JointValues jointTorque; + size_t commandedTransformations_count; + Transformation commandedTransformations[5]; + size_t writeIORequest_count; + FriIOValue writeIORequest[10]; +} MessageCommandData; + +typedef struct _FRICommandMessage { + MessageHeader header; + bool has_commandData; + MessageCommandData commandData; + bool has_endOfMessageData; + MessageEndOf endOfMessageData; +} FRICommandMessage; + +/* Default values for struct fields */ + +/* Field tags (for use in manual encoding/decoding) */ +#define CartesianVector_element_tag 1 +#define Checksum_crc32_tag 1 +#define ConnectionInfo_sessionState_tag 1 +#define ConnectionInfo_quality_tag 2 +#define ConnectionInfo_sendPeriod_tag 3 +#define ConnectionInfo_receiveMultiplier_tag 4 +#define FriIOValue_name_tag 1 +#define FriIOValue_type_tag 2 +#define FriIOValue_direction_tag 3 +#define FriIOValue_digitalValue_tag 4 +#define FriIOValue_analogValue_tag 5 +#define JointValues_value_tag 1 +#define MessageHeader_messageIdentifier_tag 1 +#define MessageHeader_sequenceCounter_tag 2 +#define MessageHeader_reflectedSequenceCounter_tag 3 +#define RobotInfo_numberOfJoints_tag 1 +#define RobotInfo_safetyState_tag 2 +#define RobotInfo_driveState_tag 5 +#define RobotInfo_operationMode_tag 6 +#define RobotInfo_controlMode_tag 7 +#define TimeStamp_sec_tag 1 +#define TimeStamp_nanosec_tag 2 +#define MessageEndOf_messageLength_tag 1 +#define MessageEndOf_messageChecksum_tag 2 +#define MessageIpoData_jointPosition_tag 1 +#define MessageIpoData_clientCommandMode_tag 10 +#define MessageIpoData_overlayType_tag 11 +#define MessageIpoData_trackingPerformance_tag 12 +#define MessageMonitorData_measuredJointPosition_tag 1 +#define MessageMonitorData_measuredTorque_tag 2 +#define MessageMonitorData_commandedJointPosition_tag 3 +#define MessageMonitorData_commandedTorque_tag 4 +#define MessageMonitorData_externalTorque_tag 5 +#define MessageMonitorData_readIORequest_tag 8 +#define MessageMonitorData_timestamp_tag 15 +#define Transformation_name_tag 1 +#define Transformation_matrix_tag 2 +#define Transformation_timestamp_tag 3 +#define FRIMonitoringMessage_header_tag 1 +#define FRIMonitoringMessage_connectionInfo_tag 4 +#define FRIMonitoringMessage_robotInfo_tag 2 +#define FRIMonitoringMessage_monitorData_tag 3 +#define FRIMonitoringMessage_ipoData_tag 5 +#define FRIMonitoringMessage_requestedTransformations_tag 6 +#define FRIMonitoringMessage_endOfMessageData_tag 15 +#define MessageCommandData_jointPosition_tag 1 +#define MessageCommandData_cartesianWrenchFeedForward_tag 2 +#define MessageCommandData_jointTorque_tag 3 +#define MessageCommandData_commandedTransformations_tag 4 +#define MessageCommandData_writeIORequest_tag 5 +#define FRICommandMessage_header_tag 1 +#define FRICommandMessage_commandData_tag 2 +#define FRICommandMessage_endOfMessageData_tag 15 + +/* Struct field encoding specification for nanopb */ +extern const pb_field_t JointValues_fields[2]; +extern const pb_field_t TimeStamp_fields[3]; +extern const pb_field_t CartesianVector_fields[2]; +extern const pb_field_t Checksum_fields[2]; +extern const pb_field_t Transformation_fields[4]; +extern const pb_field_t FriIOValue_fields[6]; +extern const pb_field_t MessageHeader_fields[4]; +extern const pb_field_t ConnectionInfo_fields[5]; +extern const pb_field_t RobotInfo_fields[6]; +extern const pb_field_t MessageMonitorData_fields[8]; +extern const pb_field_t MessageIpoData_fields[5]; +extern const pb_field_t MessageCommandData_fields[6]; +extern const pb_field_t MessageEndOf_fields[3]; +extern const pb_field_t FRIMonitoringMessage_fields[8]; +extern const pb_field_t FRICommandMessage_fields[4]; + +/* Maximum encoded size of messages (where known) */ +#define TimeStamp_size 12 +#define CartesianVector_size 54 +#define Checksum_size 11 +#define Transformation_size 188 +#define FriIOValue_size 98 +#define MessageHeader_size 18 +#define ConnectionInfo_size 24 +#define MessageEndOf_size 24 + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif diff --git a/src/iiwa_controller/external/libFRI/src/protobuf_gen/Makefile b/src/iiwa_controller/external/libFRI/src/protobuf_gen/Makefile new file mode 100644 index 0000000..863257a --- /dev/null +++ b/src/iiwa_controller/external/libFRI/src/protobuf_gen/Makefile @@ -0,0 +1,15 @@ +BASE_DIR = ../.. +include $(BASE_DIR)/build/GNUMake/paths.mak +include $(BASE_DIR)/build/GNUMake/$(TOOLS_MAK) + +CC_SRC = FRIMessages.pb.c + +INC_DIR += $(NANOPB_DIR) +CFLAGS += +LDFLAGS += + +################################################################################ +### Include general makefile (at the end) +################################################################################ + +include $(BASE_DIR)/build/GNUMake/rules.mak diff --git a/src/iiwa_controller/iiwa_controller_plugin.xml b/src/iiwa_controller/iiwa_controller_plugin.xml new file mode 100644 index 0000000..5d2fb0a --- /dev/null +++ b/src/iiwa_controller/iiwa_controller_plugin.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/src/iiwa_controller/include/iiwa_controller/FRIClient.h b/src/iiwa_controller/include/iiwa_controller/FRIClient.h new file mode 100644 index 0000000..eb642cc --- /dev/null +++ b/src/iiwa_controller/include/iiwa_controller/FRIClient.h @@ -0,0 +1,29 @@ +#pragma once + +#include +#include +#include +#include + +class FRIClient : public KUKA::FRI::LBRClient { + + public: + FRIClient(); + + void monitor() override; + void waitForCommand() override; + void command() override; + void onStateChange(KUKA::FRI::ESessionState oldState, + KUKA::FRI::ESessionState newState) override; + + std::array getMeasuredJointPositions() const; + std::array getMeasuredTorque() const; + + void setTargetJointPositions(const std::array target_pos); + + private: + std::array measuredJointPositions_; + std::array measuredTorque_; + std::array targetJointPositions_; + +}; diff --git a/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp new file mode 100644 index 0000000..51d3cc2 --- /dev/null +++ b/src/iiwa_controller/include/iiwa_controller/IIWAHardwareInterface.hpp @@ -0,0 +1,57 @@ +#ifndef IIWA_HARDWARE_INTERFACE_HPP +#define IIWA_HARDWARE_INTERFACE_HPP + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "rclcpp/macros.hpp" +#include "FRIClient.h" +#include "friUdpConnection.h" +#include "friClientApplication.h" + +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using namespace KUKA::FRI; + +namespace iiwa_controller +{ + + class IIWAHardwareInterface : public hardware_interface::SystemInterface { + + public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + private: + // TODO: append robotClient FRI + std::unique_ptr fri_client_; + std::unique_ptr app_; + std::unique_ptr connection_; + + bool simulate_; + std::string hw_command_mode_; + std::vector hw_commands_; + std::vector hw_states_position_; + std::vector hw_states_velocity_; + std::vector hw_states_effort_; + std::vector internal_command_position; + std::vector prev_measured_pos_; + bool safety_override_active_ = true; + }; + + +} + + + +#endif \ No newline at end of file diff --git a/src/iiwa_controller/package.xml b/src/iiwa_controller/package.xml new file mode 100644 index 0000000..3f9004f --- /dev/null +++ b/src/iiwa_controller/package.xml @@ -0,0 +1,22 @@ + + + + iiwa_controller + 0.1.0 + Own controller for controlling the KUKA aiwa 7 collaborative robot using the FRI library + Daniell + Apache-2.0 + + ament_cmake + + hardware_interface + pluginlib + rclcpp + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/iiwa_controller/src/FRIClient.cpp b/src/iiwa_controller/src/FRIClient.cpp new file mode 100644 index 0000000..3a4f9b6 --- /dev/null +++ b/src/iiwa_controller/src/FRIClient.cpp @@ -0,0 +1,79 @@ +#include "iiwa_controller/FRIClient.h" +#include "rclcpp/rclcpp.hpp" + +using namespace KUKA::FRI; + +inline const char* to_string(KUKA::FRI::ESessionState s) +{ + using namespace KUKA::FRI; + switch (s) + { + case IDLE: return "IDLE"; + case MONITORING_WAIT: return "MONITORING_WAIT"; + case MONITORING_READY: return "MONITORING_READY"; + case COMMANDING_WAIT: return "COMMANDING_WAIT"; + case COMMANDING_ACTIVE: return "COMMANDING_ACTIVE"; + default: return "UNKNOWN"; + } +} + +FRIClient::FRIClient() { + targetJointPositions_.fill(0.0); + measuredJointPositions_.fill(0.0); + measuredTorque_.fill(0.0); +}; + +void FRIClient::monitor() +{ + std::memcpy(measuredJointPositions_.data(), + robotState().getMeasuredJointPosition(), + 7 * sizeof(double)); + + std::memcpy(measuredTorque_.data(), + robotState().getMeasuredTorque(), + 7 * sizeof(double)); +} + +void FRIClient::setTargetJointPositions(const std::array target_pos) { + targetJointPositions_ = target_pos; +} + +std::array FRIClient::getMeasuredJointPositions() const { + return measuredJointPositions_; +} + +std::array FRIClient::getMeasuredTorque() const { + return measuredTorque_; +} + +void FRIClient::onStateChange(ESessionState oldState, ESessionState newState) { + RCLCPP_INFO_STREAM( + rclcpp::get_logger("FRIClient"), + "[FRI Client] FRI state: " << to_string(oldState) << " --> " << to_string(newState)); +} + + +void FRIClient::waitForCommand() +{ + std::memcpy(targetJointPositions_.data(), + robotState().getMeasuredJointPosition(), + 7 * sizeof(double)); + + std::memcpy(measuredJointPositions_.data(), + robotState().getMeasuredJointPosition(), + 7 * sizeof(double)); + + std::memcpy(measuredTorque_.data(), + robotState().getMeasuredTorque(), + 7 * sizeof(double)); + + robotCommand().setJointPosition(targetJointPositions_.data()); +} + +void FRIClient::command() { + std::memcpy(measuredJointPositions_.data(), + robotState().getMeasuredJointPosition(), + 7 * sizeof(double)); + + robotCommand().setJointPosition(targetJointPositions_.data()); +} \ No newline at end of file diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp new file mode 100644 index 0000000..e590aec --- /dev/null +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -0,0 +1,318 @@ +#include "iiwa_controller/IIWAHardwareInterface.hpp" +#include "rclcpp/rclcpp.hpp" + +using namespace KUKA::FRI; + +namespace iiwa_controller { + + template + constexpr const T& clamp(const T& v, const T& lo, const T& hi) + { + return (v < lo) ? lo : (hi < v) ? hi : v; + } + + CallbackReturn IIWAHardwareInterface::on_init(const hardware_interface::HardwareInfo & info) { + + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + return CallbackReturn::ERROR; + + simulate_ = false; + hw_states_position_.resize(info_.joints.size(), 0.0); + hw_states_velocity_.resize(info_.joints.size(), 0.0); + hw_states_effort_.resize(info_.joints.size(), 0.0); + hw_commands_.resize(info_.joints.size(), 0.0); + prev_measured_pos_.resize(info_.joints.size(), 0.0); + internal_command_position.resize(info_.joints.size(), 0.0); + + // пробегаемся по всем интерфейсам и смотрим какой режим управления установлен + for (const hardware_interface::ComponentInfo & joint : info_.joints) { + + // проверка, на то что все суставы используют один и тот же тип управления + if (joint.command_interfaces.size() != 1) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' has %li command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); + return CallbackReturn::ERROR; + } + + // что у каждого сустава ровно 3 интерфейса состояния: + if (hw_command_mode_.empty()) { + hw_command_mode_ = joint.command_interfaces[0].name; + + if (hw_command_mode_ != hardware_interface::HW_IF_POSITION && + hw_command_mode_ != hardware_interface::HW_IF_VELOCITY && + hw_command_mode_ != hardware_interface::HW_IF_EFFORT) + { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' have %s unknown command interfaces.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str()); + return CallbackReturn::ERROR; + } + } + + // + if (hw_command_mode_ != joint.command_interfaces[0].name) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' has %s command interfaces. Expected %s.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hw_command_mode_.c_str()); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces.size() != 3) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' has %li state interface. 3 expected.", joint.name.c_str(), + joint.state_interfaces.size()); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY); + return CallbackReturn::ERROR; + } + + if (joint.state_interfaces[2].name != hardware_interface::HW_IF_EFFORT) { + RCLCPP_FATAL( + rclcpp::get_logger("IiwaFRIHardwareInterface"), + "Joint '%s' have %s state interface. '%s' expected.", joint.name.c_str(), + joint.state_interfaces[0].name.c_str(), hardware_interface::HW_IF_EFFORT); + return CallbackReturn::ERROR; + } + + } + + return CallbackReturn::SUCCESS; + + } + + std::vector IIWAHardwareInterface::export_state_interfaces() { + std::vector state_interfaces; + for (uint i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_states_position_[i])); + } + + for (uint i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_states_velocity_[i])); + } + + for (uint i = 0; i < info_.joints.size(); i++) { + state_interfaces.emplace_back( + hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_states_effort_[i])); + } + + return state_interfaces; + } + + std::vector IIWAHardwareInterface::export_command_interfaces() { + std::vector command_interfaces; + + for (uint i = 0; i < info_.joints.size(); i++) { + if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &hw_commands_[i])); + + } else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &hw_commands_[i])); + + } else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) { + command_interfaces.emplace_back( + hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &hw_commands_[i])); + } + + } + + return command_interfaces; + + } + + CallbackReturn IIWAHardwareInterface::on_activate(const rclcpp_lifecycle::State& ) { + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Starting ...please wait..."); + + auto it = info_.hardware_parameters.find("simulate"); + if (it != info_.hardware_parameters.end()) { + std::string sim_str = it->second; + std::transform(sim_str.begin(), sim_str.end(), sim_str.begin(), ::tolower); + simulate_ = (sim_str == "true"); + } + + if (!simulate_) { + + std::string ip = info_.hardware_parameters.at("robot_ip"); + int port = std::stoi(info_.hardware_parameters.at("robot_port")); + + fri_client_ = std::make_unique(); + connection_ = std::make_unique(); + app_ = std::make_unique(*connection_, *fri_client_); + app_->connect(port, ip.c_str()); + + rclcpp::Time now = rclcpp::Clock().now(); + rclcpp::Duration period = rclcpp::Duration::from_seconds(0.01); + this->read(now, period); + + safety_override_active_ = true; + hw_commands_ = hw_states_position_; + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Connecting FRI to port= %i and ip= %s", port, ip.c_str()); + + } + + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System Successfully started!"); + + return CallbackReturn::SUCCESS; + + } + + CallbackReturn IIWAHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& ) { + + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "Stopping ...please wait..."); + + if (!simulate_) { + app_->disconnect(); + } + + std::fill(hw_commands_.begin(), hw_commands_.end(), 0.0); + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "hw_commands_ reset to zero."); + + + RCLCPP_INFO(rclcpp::get_logger("IiwaFRIHardwareInterface"), "System successfully stopped!"); + + return CallbackReturn::SUCCESS; + } + + + hardware_interface::return_type IIWAHardwareInterface::read(const rclcpp::Time&, const rclcpp::Duration& period) { + if (!simulate_) { + + if (!app_ || !app_->step()) // session порвалась? + { + RCLCPP_ERROR(rclcpp::get_logger("IIWAHardwareInterface"), + "FRI session lost"); + return hardware_interface::return_type::ERROR; + } + + /* ---------- 2. Считываем измеренные данные ---------- */ + const auto pos_meas = fri_client_->getMeasuredJointPositions(); + const auto tau_meas = fri_client_->getMeasuredTorque(); + + /* ---------- 3. Копируем в ros2_control ---------- */ + for (size_t i = 0; i < hw_states_position_.size(); ++i) + { + hw_states_position_[i] = pos_meas[i]; + + // простая численная производная = (dq) / dt + hw_states_velocity_[i] = + (pos_meas[i] - prev_measured_pos_[i]) / period.seconds(); + + hw_states_effort_[i] = tau_meas[i]; + prev_measured_pos_[i] = pos_meas[i]; + } + + return hardware_interface::return_type::OK; + } + + for (size_t i = 0; i < hw_states_position_.size(); ++i) { + hw_states_position_[i] = hw_commands_[i]; + hw_states_velocity_[i] = 0.0; + hw_states_effort_[i] = 0.0; + } + return hardware_interface::return_type::OK; + + } + + hardware_interface::return_type IIWAHardwareInterface::write(const rclcpp::Time&, const rclcpp::Duration&) + { + if (simulate_) + { + RCLCPP_DEBUG( + rclcpp::get_logger("IIWAHardwareInterface"), + "Simulated write to robot (echo commands)"); + return hardware_interface::return_type::OK; + } + + // ---------- 1. Подготовка массивов команд ---------- + std::array cmd_position{}; + std::array cmd_torque{}; + + for (size_t i = 0; i < hw_commands_.size(); ++i) + { + if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) + cmd_position[i] = hw_commands_[i]; + else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) + cmd_torque[i] = hw_commands_[i]; + } + + // ---------- 2. Проверка на "нулевые" команды ---------- + double sum = std::accumulate( + hw_commands_.begin(), hw_commands_.end(), 0.0, + [](double a, double b) { return a + std::abs(b); }); + + if (sum > 1e-3 && safety_override_active_) + { + RCLCPP_WARN_ONCE( + rclcpp::get_logger("IIWAHardwareInterface"), + "Command ignored: hw_commands_ are effectively zero (likely startup or stale)"); + return hardware_interface::return_type::OK; + } + + safety_override_active_ = false; + + // ---------- 3. Защита по лимитам углов ---------- + const double joint_limits[7][2] = { + {-2.95, 2.95}, {-2.03, 2.03}, {-2.95, 2.95}, + {-2.03, 2.03}, {-2.95, 2.95}, {-2.03, 2.03}, {-3.0, 3.0}}; + + if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) + { + for (size_t i = 0; i < 7; ++i) + { + cmd_position[i] = clamp(cmd_position[i], joint_limits[i][0], joint_limits[i][1]); + } + } + + // ---------- 4. Отправка команды в FRI-клиент ---------- + if (hw_command_mode_ == hardware_interface::HW_IF_POSITION) + { + fri_client_->setTargetJointPositions(cmd_position); + } + else if (hw_command_mode_ == hardware_interface::HW_IF_EFFORT) + { + // TODO: реализовать setTargetTorque при необходимости + } + else if (hw_command_mode_ == hardware_interface::HW_IF_VELOCITY) + { + // Velocity mode не реализован в FRI + } + + RCLCPP_DEBUG( + rclcpp::get_logger("IIWAHardwareInterface"), + "Command sent to FRI"); + return hardware_interface::return_type::OK; + } + +} + +#include + +PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)