Добавлена реализация контроллера, для работы с физическим роботом
This commit is contained in:
Даниил Грабарь
@@ -1,3 +1,11 @@
|
||||
Подмена файла по пути обязательна: `/opt/ros/rolling/lib/webots_ros2_driver/ros2_supervisor.py`
|
||||
|
||||
> необходимо `warn` заменить на `warning` в логере
|
||||
> необходимо `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-* \
|
||||
```
|
||||
@@ -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()
|
||||
@@ -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.
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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 <vector>
|
||||
#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.
|
||||
* <br>
|
||||
* 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.
|
||||
*
|
||||
* <b>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.</b>
|
||||
*/
|
||||
class TransformationClient
|
||||
{
|
||||
|
||||
friend class ClientApplication;
|
||||
|
||||
public:
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
* \brief Constructor.
|
||||
**/
|
||||
TransformationClient();
|
||||
|
||||
/** <br> <b>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.</b> <br>
|
||||
* \brief Virtual destructor.
|
||||
**/
|
||||
virtual ~TransformationClient();
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
* \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;
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
* \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<const char*>& getRequestedTransformationIDs() const;
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
*
|
||||
* \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;
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
* \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;
|
||||
|
||||
/**
|
||||
* <br> <b>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.</b> <br>
|
||||
* \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: <br>
|
||||
* [Transformation(3x4)] = [Rotation(3x3) | Translation(3x1) ]
|
||||
* <p>
|
||||
* 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()).
|
||||
* <p>
|
||||
* 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
|
||||
@@ -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 <cstdlib>
|
||||
|
||||
#ifdef _WIN32
|
||||
#include <winsock2.h>
|
||||
#else
|
||||
// if linux or a other unix system is used, select uses the following include
|
||||
#ifdef __unix__
|
||||
#include <sys/select.h>
|
||||
#endif
|
||||
// for VxWorks
|
||||
#ifdef VXWORKS
|
||||
#include <selectLib.h>
|
||||
#include <sockLib.h>
|
||||
#endif
|
||||
#include <netinet/in.h>
|
||||
#include <arpa/inet.h>
|
||||
#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
|
||||
@@ -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
|
||||
@@ -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 <cstdio>
|
||||
#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;
|
||||
}
|
||||
|
||||
@@ -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 <vector>
|
||||
|
||||
#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<const char*> 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
|
||||
@@ -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
|
||||
@@ -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 <cstdio>
|
||||
#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;
|
||||
}
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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; i<NUMBER_OF_JOINTS; i++)
|
||||
{
|
||||
int state = (int)values->value[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<const char*>& LBRState::getRequestedIO_IDs() const
|
||||
{
|
||||
return _clientData->getRequestedIO_IDs();
|
||||
}*/
|
||||
@@ -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
|
||||
+191
@@ -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 <cstring>
|
||||
#include <cstdio>
|
||||
|
||||
#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<const char*>& TransformationClient::getRequestedTransformationIDs() const
|
||||
{
|
||||
unsigned int trafoCount = _data->monitoringMsg.requestedTransformations_count;
|
||||
_data->requestedTrafoIDs.resize(trafoCount);
|
||||
for (unsigned int i=0; i<trafoCount; i++)
|
||||
{
|
||||
_data->requestedTrafoIDs[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<const char*>& TransformationClient::getRequestedIO_IDs() const
|
||||
{
|
||||
return _data->getRequestedIO_IDs();
|
||||
}*/
|
||||
@@ -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
|
||||
@@ -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 <cstring>
|
||||
#include <cstdio>
|
||||
#ifndef _MSC_VER
|
||||
#include <unistd.h>
|
||||
#endif
|
||||
|
||||
#include "friUdpConnection.h"
|
||||
|
||||
|
||||
#ifdef WIN32
|
||||
#include <winsock2.h>
|
||||
#include <Ws2tcpip.h>
|
||||
#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<socklen_t>(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<char*>(buffer), size, 0, (struct sockaddr *)&_controllerAddr, sizeof(_controllerAddr));
|
||||
if (sent == size)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
@@ -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.
|
||||
@@ -0,0 +1,20 @@
|
||||
Copyright (c) 2011 Petteri Aimonen <jpa at nanopb.mail.kapsi.fi>
|
||||
|
||||
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.
|
||||
@@ -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
|
||||
@@ -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.
|
||||
|
||||
|
||||
@@ -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 <stdint.h>
|
||||
#include <stddef.h>
|
||||
#include <stdbool.h>
|
||||
#include <string.h>
|
||||
|
||||
#ifdef PB_ENABLE_MALLOC
|
||||
#include <stdlib.h>
|
||||
#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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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
|
||||
@@ -0,0 +1,671 @@
|
||||
/* pb_encode.c -- encode a protobuf using minimal resources
|
||||
*
|
||||
* 2011 Petteri Aimonen <jpa@kapsi.fi>
|
||||
*/
|
||||
|
||||
#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);
|
||||
}
|
||||
|
||||
@@ -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
|
||||
@@ -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 <types/vxTypes.h> // 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 <stdint.h>
|
||||
#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 <stddef.h>
|
||||
#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 <stdbool.h>
|
||||
#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 <stdlib.h>
|
||||
#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 <string.h>
|
||||
#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_
|
||||
@@ -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 <stdint.h>
|
||||
#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 <stddef.h>
|
||||
#else
|
||||
|
||||
//typedef uint32_t size_t; // wird dieser typedef wirklich benötigt?!?
|
||||
#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 <stdbool.h>
|
||||
#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 <stdlib.h>
|
||||
#else
|
||||
void *realloc(void *ptr, size_t size);
|
||||
void free(void *ptr);
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* string.h subset */
|
||||
#ifdef HAVE_STRING_H
|
||||
#include <string.h>
|
||||
#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 <string.h>
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
#endif
|
||||
@@ -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
|
||||
+121
@@ -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 <cstdio>
|
||||
#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;
|
||||
}
|
||||
@@ -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
|
||||
+145
@@ -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 <cstdio>
|
||||
#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;
|
||||
}
|
||||
+133
@@ -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
|
||||
@@ -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 <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#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);
|
||||
}
|
||||
@@ -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
|
||||
@@ -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)
|
||||
|
||||
@@ -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 <pb.h>
|
||||
|
||||
#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
|
||||
@@ -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
|
||||
@@ -0,0 +1,6 @@
|
||||
<library path="iiwa_controller">
|
||||
<class name="iiwa_controller/IIWAHardwareInterface"
|
||||
type="iiwa_controller::IIWAHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface"/>
|
||||
</library>
|
||||
|
||||
@@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
|
||||
#include <friLBRClient.h>
|
||||
#include <vector>
|
||||
#include <array>
|
||||
#include <cstring>
|
||||
|
||||
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<double, 7> getMeasuredJointPositions() const;
|
||||
std::array<double, 7> getMeasuredTorque() const;
|
||||
|
||||
void setTargetJointPositions(const std::array<double, 7> target_pos);
|
||||
|
||||
private:
|
||||
std::array<double, 7> measuredJointPositions_;
|
||||
std::array<double, 7> measuredTorque_;
|
||||
std::array<double, 7> targetJointPositions_;
|
||||
|
||||
};
|
||||
@@ -0,0 +1,57 @@
|
||||
#ifndef IIWA_HARDWARE_INTERFACE_HPP
|
||||
#define IIWA_HARDWARE_INTERFACE_HPP
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#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<hardware_interface::StateInterface> export_state_interfaces() override;
|
||||
std::vector<hardware_interface::CommandInterface> 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<FRIClient> fri_client_;
|
||||
std::unique_ptr<ClientApplication> app_;
|
||||
std::unique_ptr<UdpConnection> connection_;
|
||||
|
||||
bool simulate_;
|
||||
std::string hw_command_mode_;
|
||||
std::vector<double> hw_commands_;
|
||||
std::vector<double> hw_states_position_;
|
||||
std::vector<double> hw_states_velocity_;
|
||||
std::vector<double> hw_states_effort_;
|
||||
std::vector<double> internal_command_position;
|
||||
std::vector<double> prev_measured_pos_;
|
||||
bool safety_override_active_ = true;
|
||||
};
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>iiwa_controller</name>
|
||||
<version>0.1.0</version>
|
||||
<description>Own controller for controlling the KUKA aiwa 7 collaborative robot using the FRI library</description>
|
||||
<maintainer email="daniell@example.com">Daniell</maintainer>
|
||||
<license>Apache-2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>hardware_interface</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>rclcpp</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -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<double, 7> target_pos) {
|
||||
targetJointPositions_ = target_pos;
|
||||
}
|
||||
|
||||
std::array<double, 7> FRIClient::getMeasuredJointPositions() const {
|
||||
return measuredJointPositions_;
|
||||
}
|
||||
|
||||
std::array<double, 7> 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());
|
||||
}
|
||||
@@ -0,0 +1,318 @@
|
||||
#include "iiwa_controller/IIWAHardwareInterface.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
using namespace KUKA::FRI;
|
||||
|
||||
namespace iiwa_controller {
|
||||
|
||||
template<typename T>
|
||||
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<hardware_interface::StateInterface> IIWAHardwareInterface::export_state_interfaces() {
|
||||
std::vector<hardware_interface::StateInterface> 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<hardware_interface::CommandInterface> IIWAHardwareInterface::export_command_interfaces() {
|
||||
std::vector<hardware_interface::CommandInterface> 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<FRIClient>();
|
||||
connection_ = std::make_unique<UdpConnection>();
|
||||
app_ = std::make_unique<ClientApplication>(*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<double, 7> cmd_position{};
|
||||
std::array<double, 7> 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/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_controller::IIWAHardwareInterface, hardware_interface::SystemInterface)
|
||||
Reference in New Issue
Block a user